Kalman滤波在船舶GPS导航定位系统中的应用

matlab程序如下：

function GPS
clc;clear;
T=1;                    %雷达扫描周期
N=80/T;                 %总采样次数
X=zeros(4,N);           %目标真实位置、速度（X,Vx，Y,Vy）
X(:,1)=[-100,2,200,20]; %矩阵X的第一列 目标初始位置、速度
Z=zeros(2,N);           %传感器观测值（X,Y）
Z(:,1)=[X(1,1),X(3,1)]; %观测数据初始化
delta_w=1e-2;
Q=delta_w*diag([0.5,1,0.5,1]);              %diag对角矩阵
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);  %sqrtm矩阵平方根 目标真实轨迹
    Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);    %对目标观测
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,:),'-  ');              %真实轨迹
plot(Z(1,:),Z(2,:),'-b.');              %观测轨迹
plot(Xkf(1,:),Xkf(3,:),'-r+');          %卡尔曼滤波轨迹
legend('真实轨迹','观测轨迹','滤波轨迹');
xlabel('横坐标X/m');
ylabel('纵坐标X/m');

figure
hold on; box on;
plot(Err_Observation,'-ko','MarkerFace','g');
plot(Err_KalmanFilter,'-ks','MarkerFace','r');
legend('滤波前误差','滤波后误差');
xlabel('观测时间/s');
ylabel('误差值');

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

 图一 轨迹跟踪图

图二 误差跟踪图