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

1.基本原理

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

为了使得模型简单化，假定船舶出港后沿某直线航行，以码头的出发点为坐标原点。若采样时间为 T 0 T_0 T0​,用 s ( k ) s(k) s(k)表示在 k T 0 kT_0 kT0​时刻的真实位置，用 y ( k ) y(k) y(k)表示在 k T 0 kT_0 kT0​时刻GPS的定位的观测值:

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

其中 v ( k ) v(k) v(k)是观测的高斯白噪声, v ( k ) ∼ N ( 0 , σ v 2 ) v(k) \sim N(0,\sigma_v^2) v(k)∼N(0,σv2​)。若在 k T 0 kT_0 kT0​处船舶加速度为 a ( k ) a(k) a(k),由匀加速的运动公式为:

s ( k + 1 ) = s ( k ) + s ˙ ( k ) T 0 + 1 2 a ( k ) T 0 2 s ˙ ( k + 1 ) = s ˙ ( k ) + a ( k ) T 0 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 s(k+1)=s(k)+s˙(k)T0​+21​a(k)T02​s˙(k+1)=s˙(k)+a(k)T0​

其中加速度是由两部分组成的，一部分是机动的加速度 u ( k ) u(k) u(k)和随机的扰动加速度 w ( k ) w(k) w(k)组成的,其中 w ( k ) ∼ N ( 0 , σ w 2 ) w(k) \sim N(0,\sigma_w^2) w(k)∼N(0,σw2​),可以表述如下:

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

定义一个船舶系统的状态 x ( k ) x(k) x(k)为 x ( k ) = ( s ( k ) s ˙ ( k ) ) T x(k) = (s(k)\quad \dot s(k) )^T x(k)=(s(k)s˙(k))T,因此可以得到船舶的运动状态方程:

x ( k + 1 ) = ( 1 T 0 0 1 ) x ( k ) + ( 1 2 T 0 2 T 0 ) u ( k ) + ( 1 2 T 0 2 T 0 ) w ( k ) x(k+1) = \begin{pmatrix}1&T_0\\0&1 \end{pmatrix}x(k) + \begin{pmatrix}\frac{1}{2}T_0^2\\T_0 \end{pmatrix}u(k) + \begin{pmatrix}\frac{1}{2}T_0^2\\T_0 \end{pmatrix}w(k) x(k+1)=(10​T0​1​)x(k)+(21​T02​T0​​)u(k)+(21​T02​T0​​)w(k)

观测的方程为:

y ( k ) = ( 1 0 ) x ( k ) + v ( k ) y(k) = (1 \quad 0)x(k) + v(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 k k时刻的位置 x ( k ) x(k) x(k)的最优估计值 x ^ ( k ) \hat x(k) x^(k).

二维模型

将状态量改写为以下的形式: X ( k ) = ( x ( k ) x ˙ ( k ) y ( k ) y ˙ ( k ) ) T , U ( k ) = ( u x ( k ) u y ( k ) ) T X(k) = (x(k)\ \dot x(k)\ y(k)\ \dot y(k))^T,U(k) = (ux(k)\ uy(k))^T X(k)=(x(k)x˙(k)y(k)y˙​(k))T,U(k)=(ux(k)uy(k))T,那么状态方程为以下的式子：

X ( k + 1 ) = A x ( k ) + B U ( k ) + W ( k ) Z ( k + 1 ) = C X ( k + 1 ) + v 2 × 1 ( k ) X(k+1) = Ax(k) +BU(k) +W(k)\\ Z(k+1) = CX(k+1) +v_{2\times 1}(k) X(k+1)=Ax(k)+BU(k)+W(k)Z(k+1)=CX(k+1)+v2×1​(k)

其中的系数如下:

A = ( 1 T 0 0 0 0 1 0 0 0 0 1 T 0 0 0 0 1 ) B = ( 0.5 T 2 0 T 0 0 0 0.5 T 0 2 0 T 0 ) W ( k ) = B w 2 × 1 ( k ) C = ( 1 0 0 0 0 0 1 0 ) A = \begin{pmatrix} 1&T_0&0&0\\0&1&0&0\\0&0&1&T_0\\0&0&0&1\\\end{pmatrix}\\ B = \begin{pmatrix} 0.5T^2 &0\\T_0&0\\0&0.5T_0^2\\0&T_0\\\end{pmatrix}\\ W(k) = Bw_{2\times1}(k)\\ C = \begin{pmatrix} 1 &0&0&0\\0&0&1&0\\\end{pmatrix}\\ A=⎝⎜⎜⎛​1000​T0​100​0010​00T0​1​⎠⎟⎟⎞​B=⎝⎜⎜⎛​0.5T2T0​00​000.5T02​T0​​⎠⎟⎟⎞​W(k)=Bw2×1​(k)C=(10​00​01​00​)

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

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:NX(:,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:NX_(:,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 ongrid onplot(X(1,:),X(3,:),'-k'); %真实轨迹plot(Z(1,:),Z(2,:),'-b.');%观测轨迹plot(Xkf(1,:),Xkf(3,:),'-r+');%滤波后轨迹legend('真实轨迹','观测轨迹','滤波轨迹');xlabel('横坐标X/m');ylabel('纵坐标Y/m');

仿真的结果:

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