《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.3

最新推荐文章于 2025-07-08 16:27:09 发布

小凡1991

最新推荐文章于 2025-07-08 16:27:09 发布

阅读量3.7k

点赞数 2

CC 4.0 BY-SA版权

分类专栏：卡尔曼滤波原理

本文链接：https://blog.youkuaiyun.com/sinat_20265495/article/details/53305307

卡尔曼滤波原理专栏收录该内容

16 篇文章

订阅专栏

本文通过MATLAB仿真演示了Kalman滤波在船舶GPS导航定位系统中的应用。介绍了滤波器的设计过程，包括状态转移矩阵、观测矩阵等关键参数的设置，并通过实例展示了Kalman滤波如何减少导航系统中的误差。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Kalman滤波在船舶GPS导航定位系统中的应用
%  详细原理介绍及中文注释请参考：
%  《卡尔曼滤波原理及应用-MATLAB仿真》，电子工业出版社，黄小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function main
clc;clear;
T=1;
N=80/T;
X=zeros(4,N);
X(:,1)=[-100,2,200,20];
Z=zeros(2,N);
Z(:,1)=[X(1,1),X(3,1)];
delta_w=1e-2;
Q=delta_w*diag([0.5,1,0.5,1]) ;
R=100*eye(2);
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
H=[1,0,0,0;0,0,1,0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
    X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);
    Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);  % 此处有误，请修改为P54页第二句一致即可运行
end
Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);
P0=eye(4);
for i=2:N
    Xn=F*Xkf(:,i-1);
    P1=F*P0*F'+Q;
    K=P1*H'*inv(H*P1*H'+R);
    Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);
    P0=(eye(4)-K*H)*P1;
end
for i=1:N
    Err_Observation(i)=RMS(X(:,i),Z(:,i));
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));
end
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k');
plot(Z(1,:),Z(2,:),'-b.');
plot(Xkf(1,:),Xkf(3,:),'-r+');
legend('真实轨迹','观测轨迹','滤波轨迹')
figure
hold on; box on;
plot(Err_Observation,'-ko','MarkerFace','g')
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
legend('滤波前误差','滤波后误差')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dist=RMS(X1,X2);
if length(X2)<=2
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

结果：