K a l m a n Kalman Kalman滤波在船舶 G P S GPS GPS导航定位系统中的应用
全球定位系统 ( G l o b a l P o s i t i o n i n g S y s t e m , G P S ) (Global\quad Positioning \quad System,GPS) (GlobalPositioningSystem,GPS)广泛应用于军事和国际经济各领域。船舶 G P S GPS GPS导航定位系统将一台 G P S GPS GPS接收机安装在运动目标(船舶)上就可以进行导航定位计算。 G P S GPS GPS接收机可以实时收到在轨的导航卫星播发的信号,算出接受载体(船舶)的位置和速度。由于民用领域 G P S GPS GPS导航卫星播发的信号人为加入了高频振荡随机干扰信号,致使所有派生的卫星信号均产生高频抖动。为了提高定位精度,需要对 G P S GPS GPS关于船舶的位置和速度的观测信号进行滤波。在 G P S GPS GPS系统中人为加入的高频随机干扰信号可看成是 G P S GPS GPS定位的观测噪声,观测噪声强度(方差)可由 G P S GPS GPS观测信号用系统辨识方法求得。
为将模型简单化,假定船舶出港沿某直线方向航行。以港口码头的出发处为坐标原点,设采样时间为 T 0 T_0 T0,用 s ( k ) s(k) s(k)表示船舶在采样时刻 k T 0 kT_0 kT0处的真实位置,用 z ( k ) z(k) z(k)表示在时刻 k T 0 kT_0 kT0处 G P S GPS GPS定位的观测值,则有观测模型
z ( k ) = s ( k ) + v ( k ) z(k)=s(k)+v(k) z(k)=s(k)+v(k)
式中, v ( k ) v(k) v(k)表示 G P S GPS GPS定位误差(观测噪声),假设它是零均值、方差为 σ v 2 \sigma _v ^2 σv2的白噪声,方差 σ v 2 \sigma _v ^2 σv2以通过大量 G P S GPS GPS观测试验数据用统计方法获取。记在时刻 k T 0 kT_0 kT0处船舶速度为 s ˙ ( k ) \dot{s}(k) s˙(k),加速度为 a ( k ) a(k) a(k),由匀加速运动公式有
s ( k + 1 ) = s ( k ) + T 0 s ˙ ( k ) + 0.5 T 0 2 a ( k ) s(k+1)=s(k)+ T_0 \dot{s}(k)+0.5T_0 ^2 a(k) s(k+1)=s(k)+T0s˙(k)+0.5T02a(k)
s ˙ ( k + 1 ) = s ˙ ( k ) + T 0 a ( k ) \dot {s}(k+1)=\dot s(k)+T_0 a(k) s˙(k+1)=s˙(k)+T0a(k)
而加速度 a ( k ) a(k) a(k)由机动加速度 u ( k ) u(k) u(k)和随机加速度 w ( k ) w(k) w(k)两部分合成,即
a ( k ) = u ( k ) + w ( k ) a(k)=u(k)+w(k) a(k)=u(k)+w(k)
式中, u ( k ) u(k) u(k)为船舶动力系统的控制信号,它是人为输出的已知机动信号; w ( k ) w(k) w(k)是由海风和海浪引起的随机加速度,假设它是零均值、方差为的 σ w 2 \sigma _w ^2 σw2独立于 v ( k ) v(k) v(k)的白噪声。定义在采样时刻 k T 0 kT_0 kT0处系统的状态 x ( k ) x(k) x(k)为船舶的位置和速度,即
x ( k ) = [ s ( k ) s ˙ ( k ) ] x(k)= \begin{bmatrix} s(k) \\ \dot s(k) \end{bmatrix} x(k)=[s(k)s˙(k)]
可得到船舶运动的状态方程
[ s ( k + 1 ) s ˙ ( k + 1 ) ] = [ 1 T 0 0 1 ] [ s ( k ) s ˙ ( k ) ] + [ 0.5 T 0 2 T 0 ] u ( k ) + [ 0.5 T 0 2 T 0 ] w ( k ) \begin{bmatrix} s ( k + 1 ) \\ \dot s (k+1) \end{bmatrix} = \begin{bmatrix} 1&T_0 \\0&1 \end{bmatrix} \begin{bmatrix} s(k)\\ \dot s(k) \end{bmatrix} + \begin{bmatrix} 0.5T_0 ^2 \\ T_0 \end{bmatrix} u(k) + \begin{bmatrix} 0.5T_0 ^2 \\ T_0 \end{bmatrix} w(k) [s(k+1)s˙(k+1)]=[10T01][s(k)s˙(k)]+[0.5T02T0]u(k)+[0.5T02T0]w(k)
观测方程为
z ( k ) = [ 1 0 ] [ s ( k ) s ˙ ( k ) ] + v ( k ) z(k) = \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} s(k) \\ \dot s(k) \end{bmatrix} + v(k) z(k)=[10][s(k)s˙(k)]+v(k)
即系统的状态空间模型为
x ( k + 1 ) = Φ x ( k ) + B u ( k ) + Γ w ( k ) x(k+1) = \Phi x(k) + Bu(k) + \Gamma w(k) x(k+1)=Φx(k)+Bu(k)+Γw(k)
z ( k ) = H x ( k ) + v ( k ) z(k) = Hx(k) + v(k) z(k)=Hx(k)+v(k)
式中
Φ = [ 1 T 0 0 1 ] , B = Γ = [ 0.5 T 0 2 T 0 ] , H = [ 1 0 ] \Phi = \begin{bmatrix} 1 & T_0 \\ 0 & 1 \end{bmatrix} \, , \, B = \Gamma = \begin{bmatrix} 0.5T_0 ^2 \\ T_0 \end{bmatrix} \, , \, H = \begin{bmatrix} 1 & 0 \end{bmatrix} Φ=[10T01],B=Γ=[0.5T02T0],H=[10]
于是船舶 G P S GPS GPS导航定位 K a l m a n Kalman Kalman滤波问题是:基于 G P S GPS GPS观测数据 z ( 1 ) , z ( 2 ) , … , z ( k ) z(1),z(2),…,z(k) z(1),z(2),…,z(k),得到船舶在 k k k时刻的位置 s ( k ) s(k) s(k)的最优估计 s ^ ( k ∣ k ) \hat {s} (k|k) s^(k∣k)。
在不考虑机动目标自身的动力因素时 ( u ( k ) = 0 ) (u(k)=0) (u(k)=0),将匀速直线运动的船舶系统推广到四维,即
X ( k ) = [ x ( k ) x ˙ ( k ) y ( k ) y ˙ ( k ) ] T X(k) = \begin{bmatrix} x(k) & \dot{x}(k) & y(k) & \dot{y}(k) \end{bmatrix} ^T X(k)=[x(k)x˙(k)y(k)y˙(k)]T
状态包含水平方向的位置速度和纵向的位置速度,则系统方程可以用下式表示
[ x ( k ) x ˙ ( k ) y ( k ) y ˙ ( k ) ] = [ 1 T 0 0 0 1 0 0 0 0 1 T 0 0 0 1 ] [ x ( k − 1 ) x ˙ ( k − 1 ) y ( k − 1 ) y ˙ ( k − 1 ) ] + [ 0.5 T 2 0 T 0 0 0.5 T 2 0 T ] w 2 × 1 ( k ) \begin{bmatrix} x(k) \\ \dot x(k) \\ y(k) \\ \dot y(k) \end{bmatrix} = \begin{bmatrix} 1 & T & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & T \\ 0 & 0 & 0 &1 \end{bmatrix} \begin{bmatrix} x(k-1) \\ \dot x(k-1) \\ y(k-1) \\ \dot y(k-1) \end{bmatrix} + \begin{bmatrix} 0.5T^2 &0 \\ T & 0 \\ 0 & 0.5T^2 \\ 0 & T \end{bmatrix} w_{2 \times 1}(k) ⎣⎢⎢⎡x(k)x˙(k)y(k)y˙(k)⎦⎥⎥⎤=⎣⎢⎢⎡1000T100001000T1⎦⎥⎥⎤⎣⎢⎢⎡x(k−1)x˙(k−1)y(k−1)y˙(k−1)⎦⎥⎥⎤+⎣⎢⎢⎡0.5T2T00000.5T2T⎦⎥⎥⎤w2×1(k)
Z ( k ) = [ 1 0 0 0 0 0 1 0 ] [ x ( k ) x ˙ ( k ) y ( k ) y ˙ ( k ) ] + v 2 × 1 ( k ) Z (k) = \begin{bmatrix}1 & 0 & 0 & 0 \\ 0 & 0& 1&0 \end{bmatrix}\begin{bmatrix} x(k) \\ \dot x(k) \\ y(k) \\ \dot y(k) \end{bmatrix} + v _ {2 \times 1}(k) Z(k)=[10000100]⎣⎢⎢⎡x(k)x˙(k)y(k)y˙(k)⎦⎥⎥⎤+v2×1(k)
假定船舶在二维水平面上运动,初始位置为(-100m,200m),水平运动速为2m/s,垂直方向的运动速度为20m/s, G P S GPS GPS接收机的扫描周期为 T = 1 s T=1s T=1s,观测噪声的均值为0,方差为100。过程噪声越小,目标越接近匀速直线运动;反之,为曲线运动。