船舶GPS导航定位原理如图
所示，将一台 GPS接收机安装在运动目标（船舶）上就可以进行导航定位计算。GPS 接收机可以实时收到在轨的导航卫星播发的信号，计算出接收载体（船舶）的位置和速度。由于民用领域CPS导航卫星播发的信号人为加入了高频振荡随机干扰信号，致使所有派生的卫星信号均产生高频抖动。为了提高定位精度，需要对GPS关于船舶的位置和速度的观测信号进行滤波。在 GPS系统中人为加入的高频随机干扰信号可看成观测噪声、观测噪声强度（方差）可用系统辨识方法求得。
为将模型简单化.假定船舶出港沿某直线方向航行。以港口码头的出发处为坐标原点，设采样时间为T，用s（k）表示船舶在采样时刻T。处的真实位置，用y（k）表示在时刻kT。处
GPS定位的观测值，则有观测模型∶
y(k)=s(k)+v(k)y(k)=s(k)+v(k)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);%自导航,观测信息%% PredictXpre =F* Xkf( : ,k-1 ); %第一步:状态预测Ppre=F*P*F'+Q; %第二步:协方差预测%% UpdateK=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 :TErr_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)<=2dist = sqrt((X1( 1)-X2(1))^2 + (X1( 3)-X2(2))^2 );elsedist = sqrt((X1( 1)-X2(1))^2+(X1 (3)-X2( 3))^2 );endend%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%end```
