kalman滤波在船舶GPS导航定位系统的应用（含matlab代码）

       今天利用kalman滤波对船舶GPS导航定位系统进行分析。首先还是先对kalman滤波的知识进行了解。

参考内容：书籍《卡尔曼滤波原理及应用------matlab仿真》

卡尔曼知识

　　模型建立

　　　　观测方程：Z(k)=H*X(k)+V(k）；

　　　　状态方程：X(k)=A*X(k-1)+W(k-1);

　　其中，X（k）为系统在时刻k的状态，Z（k）为对应状态的测量值。W（k）为输入的白噪声（也是过程误差），V（k）为观测噪声（也是测量误差），W（k），V（k）是均值为零，方差阵各为Q和R的不相关白噪声。A为状态转移矩阵，H为观测矩阵。

　　卡尔曼滤波：

　　　　　　　　　预测                                                              校正

先验估计 ：                            卡尔曼增益：

先验协方差误差 ：      后验估计：

                                                                                      协方差:

样例分析：       

　　图为舰船GPS导航定位原理图，GPS接收机可以实时接收到在轨的导航卫星发射的信号，通过信号可以计算出船的位置和速度。由于民用GPS导航卫星发射的信号认为的加入了一些高频振荡随机干扰信号，导致产生的微信信号会产生高频抖动，所以为了提高定位精度，需对GPS关于船舶位置和速度的信息进行滤波。在GPS系统中的干扰信号可看成是GPS定位中的观测噪声。观测噪声的方差可以通过GPS观测信号求得。

 

       假设航行轨迹是沿某直线方向航行。设置采样时间为T0，用s(k)表示船舶在采样时刻kT0处的真实位置，用y(k)表示在采样时刻kT0处GPS定位的观测值，则观测模型：

y(k)=s(k)+v(k);

　　上式中v(k)表示GPS定位误差（观测噪声），可以假定其是零均值、方差为的白噪声，方差可以通过大量观测试验数据用统计方法获取。表示在时刻kT0时船舶的速度。加速度为a(k),所以由运动公式有：

 而其中的加速度a(k)由两个部分决定：机动加速度u(k)和随机加速度w(k)（其实可以看作过程误差）：

a(k)=w(k)+u(k);

式中，u(k)为船动力系统的控制信号，它是人为输出的已知的信号，w(k)是过程误差，可以看作一个零均值、方差为的独立与v(k)的白噪声。定义在采样时刻kT0处系统的状态x(k)的船舶的位置和速度，即：

可以得到船舶的运动方程为：

 

观测方程为：

可以得到系统的状态空间模型为

 

 

 

       因此船舶GPS导航定位kalman滤波问题就是：基于GPS观测数据(y(1),y(2),......,y(k)),得到船舶在k时刻的位置s(k)的最有估计。

 

在不考虑机动目标自身的动力也就是说u(k)=0,将匀速直线运动的船扩展开可以得到下面的公式。

 

状态方程包含水平距离和速度和纵向距离和速度。因此状态方程可以表示为：

 

 MATLAB仿真

　　假定船只在二维水平面上运动，初始位置为（-100，200）m，水平运动速度为2m/s，垂直方向运动速度为20m/s,GPS扫描周期为T=1s，观测噪声的均值为0，方差为100.过程噪声越小，目标越接近匀速直线运动；否则为曲线运动。

 

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

clc
clear all
close all

T=1;      %雷达扫描周期
N=80/T;   %总的采样时间
X=zeros(4,N);          %目标的真实位置和速度。状态方程的初始化
X(:,1)=[-100,2,200,20];   %目标初始位置和速度（x,vx,y,vy）
Z=zeros(2,N);          %传感器对位置的观测，观测方程初始化
Z(:,1)=[X(1,1),X(3,1)];          %观测初始化，x,y
Xkf=zeros(4,N);        %kalman滤波处理的状态，也是估计值
Xkf(:,1)=X(:,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];                 %观测矩阵
P0=eye(4);                           %协方差矩阵
I=eye(4);                            %I矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=2:N
   X(:,i)=F*X(:,i-1)+sqrtm(Q)*randn(4,1);       %目标的真实轨迹，状态方程
   Z(:,i)=H*X(:,i)+sqrtm(R)*randn(2,1);         %对目标的观测，观测方程       
end

%kalman滤波

for i=2:N
    X_pre=F*Xkf(:,i-1);                     %预测
    P_pre=F*P0*F'+Q;                        %预测误差协方差
    K=P_pre*H'*inv(H*P_pre*H'+R);           %卡尔曼增益
    Xkf(:,i)=X_pre+K*(Z(:,i)-H*X_pre);      %状态更新
    P0=(I-K*H)*P_pre;                       %协方差更新  
end
%误差分析
for i=1:N
   Err_Observation(i)=RMS(X(:,i),Z(:,i));       %真实值与观测值的误差
   Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));    %真实值与kaiman滤波的误差
end

%画图
figure
hold on;box on;
plot(X(1,:),X(3,:),'k-');        %真实轨迹
plot(Z(1,:),Z(2,:),'b-');        %测量轨迹
plot(Xkf(1,:),Xkf(3,:),'-ko');   %kalman滤波轨迹
legend("真实轨迹",'观测轨迹','kalman滤波轨迹');
xlabel('横坐标X/m');
ylabel('纵坐标Y/m');
grid on;
figure;
hold on;box on;
plot(Err_Observation,'ko-','MarkerFace','g');
plot(Err_KalmanFilter,'ks-','MarkerFace','r');
legend("滤波前误差",'滤波后误差');
xlabel('观测时间/s');
ylabel('误差值/m');

 

 　   从图1中可以看到观测轨迹与真实轨迹相比，存在明细的抖动情况，而经过kalman滤波的更加稳定一些。图2可看出真实轨迹和观测轨迹的误差还是很大的，最大误差快要接近35米，而kalman的误差相对来说会更稳定。

　　综合来看，kalman滤波虽然不能完全消除噪声，但是已经最大限度地降低噪声的影响了。

展望和总结

　　关于线性kalman滤波算法的应用到这已经结束。kalman滤波能够在线性高斯模型条件下，对目标的状态做出最优的估计，得到较好的跟踪结果。但实际情况总是存在不同的非线性，而针对非线性的情况，kalman滤波已经不能满足要求，对于非线性的情况，就需要运用kalman的延伸-------EKF（扩展kalman）。之后会对EKF进行学习和研究。

 

标签：误差,kalman,滤波,噪声,观测,matlab,GPS
来源： https://www.cnblogs.com/sbb-first-blog/p/16583077.html