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

1 原理介绍

    全球定位系统（GPS）广泛应用于军事和国民经济各领域。船舶GPS导航定位原理如下图所示。

    将一台GPS接收机安装在运动目标（船舶）上就可以进行导航定位计算。GPS接收机可以实时收到在轨的导航卫星播发的信号，算出接受载体（船舶）的位置和速度。

    由于民用领域GPS导航卫星播发的信号，算出接受载体（船舶）的位置和速度。由于民用领域GPS导航卫星播发的信号人为加入的高频振荡随机干扰信号，致使所有派生的卫星信号均产生高频抖动。

    为了提高定位精度，需要对GPS关于船舶的位置和速度的观测信号进行滤波。在GPS系统中认为加入的高频随机干扰信号可堪称是GPS定位的观测噪声。观测噪声强度（方差）可由GPS观测信号用系统辨识方法求得。

2 初值设定

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

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];    % 观测矩阵

3 Kalman状态和观测方程

状态方程：

X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);  % 目标真实轨迹

观测方程：

Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);    % 对目标观测

4 Kalman滤波

Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);    % Kalman滤波状态初始化
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); % Kalman增益
    Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);    % 状态更新
    P0=(eye(4)-K*H)*P1; % 滤波误差协方差更新
end

5 误差分析模块

for i=1:N
    Err_Observation(i)=RMS(X(:,i),Z(:,i));      %滤波前的误差
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));   %滤波后的误差
end

6 画图模块

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

7 欧氏距离子函数模块

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

8 运行结果

真实轨迹、观测轨迹与滤波轨迹对比结果

滤波前与滤波后误差对比

9 结果分析

    由运行结果中的图1可以看出：观测轨迹明显在振荡，说明测量噪声影响非常大，而经过Kalman滤波后，滤波估计比较接近目标的真实运动轨迹。

    由运行结果中的图2可以看出：位移的观测噪声最大值接近35m，对于目标运动场地（长约1800m，宽约250m）来说，这个噪声非常大，当然这也只限于仿真，实际传感器的测量误差不可能这么大。经过Kalman滤波之后，位置偏差降低到10m以下。

    Kalman滤波虽然不能完全消除噪声，但是它已经能很大限度地降低噪声的影响。

Matlab代码：https://gitee.com/uestc-eagles/kalman-filter.git