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

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

全球定位系统$(GlobalPositioningSystem，GPS)$广泛应用于军事和国际经济各领域。船舶$GPS$导航定位系统将一台$GPS$接收机安装在运动目标(船舶)上就可以进行导航定位计算。$GPS$接收机可以实时收到在轨的导航卫星播发的信号，算出接受载体（船舶）的位置和速度。由于民用领域$GPS$导航卫星播发的信号人为加入了高频振荡随机干扰信号，致使所有派生的卫星信号均产生高频抖动。为了提高定位精度，需要对$GPS$关于船舶的位置和速度的观测信号进行滤波。在$GPS$系统中人为加入的高频随机干扰信号可看成是$GPS$定位的观测噪声，观测噪声强度(方差)可由$GPS$观测信号用系统辨识方法求得。

为将模型简单化，假定船舶出港沿某直线方向航行。以港口码头的出发处为坐标原点，设采样时间为$T_0$，用$s(k)$表示船舶在采样时刻$k{T}_0$处的真实位置，用$z(k)$表示在时刻$k{T}_0$处$GPS$定位的观测值，则有观测模型

$z(k)=s(k)+v(k)$

式中，$v(k)$表示$GPS$定位误差(观测噪声)，假设它是零均值、方差为${\sigma }_v^2$的白噪声，方差${\sigma }_v^2$以通过大量$GPS$观测试验数据用统计方法获取。记在时刻$k{T}_0$处船舶速度为$\dot{s}(k)$，加速度为$a(k)$，由匀加速运动公式有

$s(k+1)=s(k)+T_0\dot{s}(k)+0.5T_0^{2}a(k)$

$\dot{s}(k+1)=\dot{s}(k)+T_{0}a(k)$

而加速度$a(k)$由机动加速度$u(k)$和随机加速度$w(k)$两部分合成，即

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

式中，$u(k)$为船舶动力系统的控制信号，它是人为输出的已知机动信号；$w(k)$是由海风和海浪引起的随机加速度，假设它是零均值、方差为的${\sigma }_w^2$独立于$v(k)$的白噪声。定义在采样时刻$k{T}_0$处系统的状态$x(k)$为船舶的位置和速度，即

$x(k)=\left[\begin{array}{c}s(k)\\ \dot{s}(k)\end{array}\right]$

可得到船舶运动的状态方程

$\left[\begin{array}{c}s(k+1)\\ \dot{s}(k+1)\end{array}\right]=\left[\begin{array}{cc}1& T_0\\ 0& 1\end{array}\right]\left[\begin{array}{c}s(k)\\ \dot{s}(k)\end{array}\right]+\left[\begin{array}{c}0.5T_0^2\\ T_0\end{array}\right]u(k)+\left[\begin{array}{c}0.5T_0^2\\ T_0\end{array}\right]w(k)$

观测方程为

$z(k)=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}s(k)\\ \dot{s}(k)\end{array}\right]+v(k)$

即系统的状态空间模型为

$x(k+1)=\Phi x(k)+Bu(k)+\Gamma w(k)$

$z(k)=Hx(k)+v(k)$

式中

$\Phi =\left[\begin{array}{cc}1& T_0\\ 0& 1\end{array}\right],B=\Gamma =\left[\begin{array}{c}0.5T_0^2\\ T_0\end{array}\right],H=\left[\begin{array}{cc}1& 0\end{array}\right]$

于是船舶$GPS$导航定位$Kalman$滤波问题是：基于$GPS$观测数据$z(1)，z(2)，\dots ，z(k)$，得到船舶在$k$时刻的位置$s(k)$的最优估计$\hat{s}(k|k)$。

在不考虑机动目标自身的动力因素时$(u(k)=0)$，将匀速直线运动的船舶系统推广到四维，即

$X(k)={\left[\begin{array}{cccc}x(k)& \dot{x}(k)& y(k)& \dot{y}(k)\end{array}\right]}^T$

状态包含水平方向的位置速度和纵向的位置速度，则系统方程可以用下式表示

$\left[\begin{array}{c}x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\end{array}\right]=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x(k-1)\\ \dot{x}(k-1)\\ y(k-1)\\ \dot{y}(k-1)\end{array}\right]+\left[\begin{array}{cc}0.5T^2& 0\\ T& 0\\ 0& 0.5T^2\\ 0& T\end{array}\right]w_{2\times 1}(k)$

$Z(k)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\end{array}\right]+v_{2\times 1}(k)$

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