kalman滤波器在GPS导航定位中的应用

1.基本原理

将GPS接收机安装在船舶上可以进行导航定位的计算，民用领域的GPS导航卫星播发的信号加入了高频振荡的随机干扰信号，是GPS定位的观测噪声。在这里，我们假设观测噪声强度可用系统辨识方法求得。

• 一维模型

为了使得模型简单化，假定船舶出港后沿某直线航行，以码头的出发点为坐标原点。若采样时间为$T_0$,用$s(k)$表示在$k{T}_0$时刻的真实位置，用$y(k)$表示在$k{T}_0$时刻GPS的定位的观测值:
$$y(k)=s(k)+v(k)$$
其中$v(k)$是观测的高斯白噪声,$v(k)\sim N(0,{\sigma }_v^2)$。若在$k{T}_0$处船舶加速度为$a(k)$,由匀加速的运动公式为:
$$\begin{gathered} s(k+1)=s(k)+\dot{s}(k)T_0+\frac{1}{2}a(k)T_0^2 \\ \dot{s}(k+1)=\dot{s}(k)+a(k)T_0 \end{gathered}$$
其中加速度是由两部分组成的，一部分是机动的加速度$u(k)$和随机的扰动加速度$w(k)$组成的,其中$w(k)\sim N(0,{\sigma }_w^2)$,可以表述如下:
$$a(k)=u(k)+w(k)$$
定义一个船舶系统的状态$x(k)$为$x(k)=(s(k)\dot{s}(k){)}^T$,因此可以得到船舶的运动状态方程:
$$x(k+1)=\left(\begin{array}{cc}1& T_0\\ 0& 1\end{array}\right)x(k)+\left(\begin{array}{c}\frac{1}{2}{T}_0^2\\ T_0\end{array}\right)u(k)+\left(\begin{array}{c}\frac{1}{2}{T}_0^2\\ T_0\end{array}\right)w(k)$$
观测的方程为:
$$y(k)=(10)x(k)+v(k)$$
接下来我们的目标就是用GPS的观测数据 { y ( k ) : y ( 1 ) , y ( 2 ) , . . y ( k ) } \{y(k):y(1),y(2),..y(k)\} {y(k):y(1),y(2),..y(k)}去得到$k$时刻的位置$x(k)$的最优估计值$\hat{x}(k)$.

• 二维模型

  将状态量改写为以下的形式:$X(k)=(x(k)\dot{x}(k)y(k)\dot{y}(k){)}^T,U(k)=(ux(k)uy(k){)}^T$,那么状态方程为以下的式子：
  $$\begin{gathered} X(k+1)=Ax(k)+BU(k)+W(k) \\ Z(k+1)=CX(k+1)+v_{2\times 1}(k) \end{gathered}$$
  其中的系数如下:
  $$\begin{gathered} A=\left(\begin{array}{cccc}1& T_0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T_0\\ 0& 0& 0& 1\end{array}\right) \\ B=\left(\begin{array}{cc}0.5T^2& 0\\ T_0& 0\\ 0& 0.5T_0^2\\ 0& T_0\end{array}\right) \\ W(k)=B{w}_{2\times 1}(k) \\ C=\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right) \end{gathered}$$

​ 假设船舶的初始位置为$(-100,200)m$,初始的水平运动速度为$2m/s$,垂直的运动速度为$20m/s$,$GPS$的扫描周期为$T=1s$,观测噪声均值为0，方差为100。过程中始终保持水平加速度为$1m/s^2$,垂置加速度为$2m/s^2$。下面将进行仿真计算。

2.Matlab仿真

基本的算法过程如下图所示:

代码实现:

clc,clear;
rng(0);%随机误差种子
T = 1;%采样步长
N = 80;%采样次数
X = zeros(4,N);%目标的真实位置和速度
X(:,1) = [-100 2 200 20]';%初始状态的估计
Z = zeros(2,N); %传感器位置的测量值
Z(:,1) = [-100 200]';%假设初始位置的预测正确
Q = 0.01*diag([0.5*T^2 T 0.5*T^2 T]);%状态噪声的协方差矩阵估计(其中sigma_w^2 = 0.01)
R = [100 0.01;100 0.01]; %观测误差的协方差矩阵
A = [1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];%状态转移矩阵
B = [0.5*T^2 0;T 0;0 0.5*T^2;0 T];
u = [1;2];
C = [1 0 0 0;0 0 1 0];
%以下是仿真得到测量值
for t = 2:N
    X(:,t) = A*X(:,t-1) + B*u + sqrtm(Q)*randn(4,1);
    Z(:,t) = C*X(:,t) + sqrtm(R)*randn(2,1);
end
%以下是kalman滤波阶段
Xkf = zeros(4,N);
Xkf(:,1) = X(:,1);
X_ = zeros(4,N);
X_(:,1) = X(:,1);
P0 = eye(4); %初始化状态值的协方差
for i = 2:N
    X_(:,i) = A*Xkf(:,i-1) + B*u;
    P_ = A*P0*A' + Q;
    Kk = P_*C'*inv(C*P_*C' + R);
    Xkf(:,i) = X_(:,i)+Kk*(Z(:,i) - C*X_(:,i));
    P0 = (eye(4) - Kk*C)*P_;    
end
%以下是画图
hold on
grid on
plot(X(1,:),X(3,:),'-k'); %真实轨迹
plot(Z(1,:),Z(2,:),'-b.');%观测轨迹
plot(Xkf(1,:),Xkf(3,:),'-r+');%滤波后轨迹
legend('真实轨迹','观测轨迹','滤波轨迹');
xlabel('横坐标X/m');
ylabel('纵坐标Y/m');

仿真的结果:

可以看出即使在一开始滤波的效果都是非常好的。