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

船舶GPS导航定位原理如图

所示，将一台 GPS接收机安装在运动目标（船舶）上就可以进行导航定位计算。GPS 接收机可以实时收到在轨的导航卫星播发的信号，计算出接收载体（船舶）的位置和速度。由于民用领域CPS导航卫星播发的信号人为加入了高频振荡随机干扰信号，致使所有派生的卫星信号均产生高频抖动。为了提高定位精度，需要对GPS关于船舶的位置和速度的观测信号进行滤波。在 GPS系统中人为加入的高频随机干扰信号可看成观测噪声、观测噪声强度（方差）可用系统辨识方法求得。
为将模型简单化.假定船舶出港沿某直线方向航行。以港口码头的出发处为坐标原点，设采样时间为T，用s（k）表示船舶在采样时刻T。处的真实位置，用y（k）表示在时刻kT。处
GPS定位的观测值，则有观测模型∶
$$y(k)=s(k)+v(k)$$

%%%%%%%%%%%%%%%%%%%%%%%%%% %% %%%%%%%%%%%%%%% %%
function KalmanForGPS % Kalman滤波在船舶GPS导航定位系统中的应用
dt = 1; %雷达扫描周期

T=80/ dt; %总的采样次数

F=[ 1, dt ,0 ,0;
    0,1,0,0;
    0,0, 1 , dt ;
    0 ,0,0,1 ] ;   %状态转移矩阵

H=[ 1,0,0,0;
    0,0,1,0]; %。观测矩阵

delta_w= 1e-2; % 如果增大这个参数,目标真实轨迹就是曲线了

Q=delta_w * diag( [ 0.5,1 ,0.5,1]) ; %过程噪声方差

R= 10 * eye( 2); %观测噪声方差

W = sqrtm( Q) * randn( 4,T); %过程噪声

V =sqrtm(R) * randn( 2,T);  %观测噪声

%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%% %%%%%%%%%%
X = zeros( 4,T); %。目标真实位置、速度

X( :,1)=[-100,2,200, 20]; %目标初始位置、速度

Z=zeros( 2,T); %传感器对位置的观测

Z( :,1)=[X( 1,1 ),X(3,1)]; %观测初始化

Xkf=zeros( 4,T); %。Kalman滤波状态初始化

Xkf( : ,1)=X( :,1 );
P=eye( 4 ); %协方差阵初始化

for k=2:T
    %船体自身运动
    X ( : ,k)=F* X( :,k- 1)+W( :,k ) ;     %目标真实轨迹

    %获取卫星数据,观测信息开始滤波
    Z( :,k)=H*X( :,k)+V( :,k);     %自导航,观测信息
    
    %% Predict
    Xpre =F* Xkf( : ,k-1 );  %第一步:状态预测
    Ppre=F*P*F'+Q; %第二步:协方差预测
    
    %% Update
    K=Ppre*H'*inv( H * Ppre * H'+R); %第三步:求增益
    Xkf( : ,k )= Xpre+K*(Z( :,k)-H * Xpre);%第四步:状态更新
    P=(eye( 4)-K* H) * Ppre ; %第五步:协方差更新

end
%误差分析
for i= 1 :T
    Err_Observation( i)= RMS(X( :,i) ,Z( : ,i));  %滤波前的误差

    Err_KalmanFilter( i)= RMS(X( :,i) ,Xkf( : ,i) );%滤波后的误差

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%画图
figure %轨迹图
hold on ; box on ; xlabel ( 'X/m') ; ylabel( 'Y/m' ) ;
plot( X( 1 ,: ) ,X( 3,: ) ,'-k'); %真实轨迹

plot( Z( 1 , :) ,Z( 2,: ) , '-b. ' ); %观测轨迹

plot ( Xkf(1 ,: ),Xkf( 3, : ) , '-r+'); %Kalman 滤波轨迹

legend('真实轨迹','观测轨迹','滤波轨迹');

figure %。误差图

hold on; box on ; xlabel( 'Time/s' ); ylabel( 'value of the deviation/ m') ;
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
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end```