CN102788976B - 高量级扩展卡尔曼滤波方法 - Google Patents

Abstract

本发明提供一种高量级扩展卡尔曼滤波方法，解决由远距离带来的系统量测方程非线性大，从而造成跟踪精度下降的问题。第一步：假设目标在二维平面内运动，分别建立目标的状态方程和量测方程；第二步：由k时刻的滤波结果得到目标的k+1时刻目标状态预测方程；第三步：计算预测协方差矩阵；第四步：计算扩展卡尔曼滤波器的增益矩阵；第五步：计算目标k+1的状态更新方程；第六步：计算目标k+1时刻的协方差更新误差；第七步：根据目标在k+1时刻的状态滤波向量和滤波误差的协方差矩阵，按第三步至第六步计算目标在k+2时刻的状态滤波向量和滤波误差的协方差矩阵，以此循环往复，最终的输出滤波结果再换回原来的量级。

Description

高量级扩展卡尔曼滤波方法

技术领域

本发明涉及一种高量级扩展卡尔曼滤波方法，属于目标运动状态估计领域。

背景技术

对目标运动状态进行估计时，学者Kalman将状态变量法引入到滤波理论中来，使状态空间描述与离散时间更新联系起来，对状态进行线性最小均方根误差估计，应用最为广泛，并称之为卡尔曼滤波方法。在雷达进行目标跟踪的过程中，由于在直角坐标系中易于对目标的运动状态进行描述，所以，目标状态方程通常是在直角坐标系中建立的。然而，对目标位置的量测通常是在极/球坐标系中得到的，即在极/球坐标系中，进行目标位置相对于雷达的距离、方位角或俯仰角（包括3D雷达的俯仰角）的量测。这就使得目标的运动状态与雷达量测值之间的关系是非线性的，雷达滤波系统必然是非线性系统，从而导致经典的Kalman滤波算法不能对目标进行跟踪。

卡尔曼滤波器是在线性高斯情况下利用最小均方误差准则（MMSE）获得目标状态估计的方法，但在实际应用中，许多情况下观测数据与目标状态参数间的关系是非线性的，即球/极坐标系和直角坐标系间的转换是非线性的，随着目标距离越远，坐标转换的非线性程度越大。

对于非线性滤波问题，至今尚未得到完善的解法。通常的处理方法是利用线性化技巧将非线性问题转化为近似的线性滤波问题，然后套用线性滤波理论得到求解原非线性滤波问题的次优滤波算法，其最常用的线性化方法是一阶泰勒级数展开，所得到的滤波方法是扩展卡尔曼滤波器（EKF）。然而，当目标相对于雷达的距离很远时，雷达极坐标的量测与目标在直角坐标系下的状态间的非线性会增大，导致传统的基于一阶泰勒展开的EKF发散，而SimonJulier提出的UKF[i]也会在非高斯噪声强非线性系统下无法很好的跟踪目标。

发明目的

本发明的目的在于解决由远距离带来的系统量测方程非线性大，从而造成跟踪精度下降的问题。

本发明的高量级扩展卡尔曼滤波方法，包括以下步骤：

第一步：假设目标在二维平面内运动，分别建立目标的状态方程和量测方程，如式（1）和式（2）所示；

X(k+1)＝F(k)X(k)+V(k)   （1）

$\begin{array}{c}Z(k+1)=h\left(X(k+1)\right)+W(k+1)\\ =\left[\begin{array}{c}\sqrt{x^2(k+1)+y^2(k+1)}\\ \mathrm{arcta}[y(k+1)/x(k+1)]\end{array}\right]+W(k+1)\end{array}---\left(2\right)$

其中X(k)是目标k时刻的状态向量，F(k)是状态转移矩阵，V(k)是目标运动的过程噪声，其协方差矩阵用Q(k)表示；Z(k+1)表示目标在k+1时刻的量测值，二维平面内包含两种量测量，即目标相对雷达的距离和方位角，分别用

和arctan[y(k+1)/x(k+1)]表示，x(k+1)和y(k+1)是k+1目标在直角坐标系下x轴和y轴上的位置分量，W(k+1)是k+1时刻雷达的量测误差；

第二步：根据经典一阶扩展卡尔曼滤波算法，由k时刻的滤波结果得到目标的k+1时刻目标状态预测方程

$\hat{X}(k+1|k)=F\left(k\right)\hat{X}\left(k\right|k)---\left(3\right)$

其中

表示k时刻的目标滤波向量，

中的元素采用高量级单位，在数值上变小；

第三步：计算预测协方差矩阵

P(k+1|k)＝F(k)P(k|k)F^T(k)+Q(k)   （4）

其中P(k|k)是扩展卡尔曼滤波器k时刻滤波误差的协方差矩阵；

第四步：计算扩展卡尔曼滤波器的增益矩阵

$K(k+1)=P(k+1|k)h_X^T(k+1){[h_X^T\left(k\right)P\left(k\right|k-1)h_X^T\left(k\right)+R\left(k\right)]}^{-1}---\left(5\right)$

其中，h_X(k+1)是雅克比矩阵，表示为

$h_X(k+1)={{\left[{\▿}_X{h}^{\′}(k+1,X)\right]}^{\′}}_{X=\hat{X}(k+1|k)}---\left(6\right)$

第五步：计算目标k+1的状态更新方程

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)[Z(k+1)-h[k+1,\hat{X}(k+1|k)\left]\right]---\left(7\right)$

量测值Z(k+1)使用高量级单位，数值上变小；

第六步：计算目标k+1时刻的协方差更新误差

P(k+1|k+1)＝P(k+1|k)-K(k+1)h_X(k+1)P(k+1|k)   （8）

第七步：根据目标在k+1时刻的状态滤波向量

和滤波误差的协方差矩阵P(k+1|k+1)，按第三步至第六步计算目标在k+2时刻的状态滤波向量

和滤波误差的协方差矩阵P(k+2|k+2)，以此循环往复，最终的输出滤波结果再换回原来的量级。

本发明的有益效果：本发明将状态的初始滤波值

中的元素和距离的量测值均用高量级表示，因为使用了更高的量级，滤波值和距离量测值在数值上减小了，也就是将实际中目标的远距离转换成数据处理中的“近距离”。距离在数值上的减小降低了滤波中坐标转换的非线性，从而在使用扩展卡尔曼滤波时线性化误差减小，滤波精度比没有使用量级转换的经典扩展卡尔曼滤波要高。

具体实施方式

本发明首先阐述坐标转换非线性度量问题，假设目标是在二维平面内运动，目标的观测值是在雷达极坐标下得到的，假设雷达极坐标的原点与直角坐标系的原点重合，且目标相对于雷达的真实距离为r₀，真实方位角为a₀，实际中，由于雷达的测角精度与测角精度的限制，雷达并不能获得目标真实的距离和方位角，而是包含量测误差的目标观测值，那么，目标在雷达极坐标下的量测值为，

$\left\{\begin{array}{c}r=r_0+n_r\\ a=a_0+n_a\end{array}\right.---\left(1\right)$

其中，距离维量测噪声n_r和方位维量测噪声n_a的量测噪声通常是统计独立的零均值的高斯白噪声。

通常目标状态是在直角坐标系下描述的，从而导致目标状态与雷达量测间的对应关系是非线性的。将极坐标系下的量测值转换到直角坐标系，得直角坐标系下的观测为，

$\left\{\begin{array}{c}x=r\cos a\\ y=r\sin a\end{array}\right.---\left(2\right)$

而且坐标转换是有偏的，其偏差为，

$\mathrm{bias}\left(x\right)=E\left(x\right)-x_0=x_0[\exp (-\frac{1}{2}{\mathrm{\σ}}_a^2)-1]---\left(3\right)$

$\mathrm{bias}\left(y\right)=E\left(y\right)-y_0=y_0[\exp (-\frac{1}{2}{\mathrm{\σ}}_a^2)-1]---\left(4\right)$

本发明经过以上分析，可知由于坐标转换的非线性，使得坐标转换是有偏的。

本发明对极-直坐标转换曲率进行分析，若目标运动方程为y＝f(x)，且f(x)具有二阶导数，则表征目标运动曲线的曲率公式如下，

$\mathrm{\κ}=\frac{\left|y^{\′\′}\right|}{{(1+y^{\′2})}^{3/2}}---\left(5\right)$

其中，曲率κ满足0≤κ＜+∞，根据其含义可知，当κ越大时则目标运动的非线性程度越大，而当κ＝0时，目标做直线运动，非线性程度最小。

将式（2）关于目标方位角a进行求曲率，得

${\mathrm{\κ}}_x=\frac{\left|r\cos a\right|}{{(1+r^2{\sin }^{2}a)}^{3/2}}---\left(6\right)$

${\mathrm{\κ}}_y=\frac{\left|r\sin a\right|}{{(1+r^2{\cos }^{2}a)}^{3/2}}---\left(7\right)$

将式（6）取平方并对目标距离r求偏导，可知，当0＜r≤r_0x时，κ_x随着r的增加而增加；当r≥r_0x时，κ_x随着r的增加而减小。同理，当0＜r≤r_0y时，κ_y随着r的增加而增加；当r≥r_0y时，κ_y随着r的增加而减小。其中，

$r_{0x}=\sqrt{1/\left(2{\sin }^{2}a\right)}---\left(8\right)$

$r_{0y}=\sqrt{1/\left(2{\cos }^{2}a\right)}---\left(9\right)$

定义雷达量测二维极-直坐标转换函数的曲率为

κ＝max(κ_x,κ_y)   （10）

当0＜r≤r_0κ时，κ随着r的增加而增加，当r≥r_0x时，κ随着r的增加而减小。其中，

r_0κ＝max(r_0x,r_0y)   （11）

当目标方位角位于坐标轴附近时，r_0κ→∞，即随着目标距离的增加，曲率增大，进而非线性也会变大，可以采用高量级的单位表示目标距离。

通过对二维坐标转换非线性成因的分析可知，随着目标距离的增加，雷达极坐标系下的量测与对应的直角坐标系下的目标状态间的非线性关系增大。本发明针对远距离目标，提出使用高量级扩展卡尔曼滤波（HEKF）的方法对远距离目标进行跟踪，从而降低远距离的影响，提高远距离目标跟踪精度。

本发明描述的状态空间模型包括状态方程和量测方程，状态方程用来描述状态随时间演变的过程，量测方程表示雷达量测与目标状态间的关系。

状态方程设为

X(k+1)＝f(X(k))+V(k)   （12）

其中，X(k)为k时刻目标运动的状态矢量。f(·)为目标运动状态转移函数，当目标运动是非线性的，此函数呈现非线性。并假定过程噪声V(k)是加性零均值白噪声，其方差为，

E[V(k)V^T(j)]＝Q(k)δ_kj   （13）

量测方程为，

Z k+1=h X k+1+W k+1   （14）

其中，h(·)为量测函数。一般情况下，由于雷达极坐标量测与直角坐标状态间的非线性关系，h(·）是非线性的。并假定量测噪声W(k+1)为加性零均值的高斯白噪声，其方差为，

E(W(k+1)W^T(j))＝R(k+1)δ_kj   （15）

贝叶斯滤波理论是基于上述状态空间模型，利用所有已知信息来构造目标状态的后验概率密度，进而估计目标状态的最优方法。当状态空间模型呈现非线性时，EKF是贝叶斯估计的一种次优实现方法。它使用泰勒级数展开的一次项对非线性函数进行线性近似，然后在Kalman滤波框架下进行目标状态估计。

本发明考虑二维平面极坐标量测方程的情况，此时k+1时刻的雷达量测向量Z(k+1)＝[r(k+1) a(k+1)]^T，其中r(k+1)和a(k+1)分别为目标径向距离和方位角。相应的量测函数为，

$h\left(X(k+1)\right)=\left[\begin{array}{c}\sqrt{x^2(k+1)+y^2(k+1)}\\ \arctan [y(k+1)/x(k+1)]\end{array}\right]---\left(16\right)$

显然式（16）是式（2）的逆变换，因而式（16）坐标转换的非线性是与式（2）式相对应的。若极坐标对直角坐标的坐标转换的非线性程度不高，则相应的直角坐标对极坐标的坐标转换的非线性程度亦不高，反之亦然。

本发明对目标运动状态进行估计时，卡尔曼滤波算法将状态变量法引入到滤波理论中来，将状态空间描述与离散时间更新联系起来，对状态进行线性最小均方根误差估计，应用最为广泛。在非线性滤波系统中，一阶扩展卡尔曼滤波（EKF）应用较多，EKF使用非线性方程的一阶泰勒展开来近似非线性方程，将非线性滤波系统进行线性化处理，从而可以将传统的卡尔曼滤波应用到非线性系统，得到目标状态的估计。

当目标相对雷达的距离较远时，传统的一阶扩展卡尔曼滤波（EKF）算法随着线性化误差的积累而发散，因此针对远距离目标，对一阶扩展卡尔曼滤波算法进行改进，得到高量级扩展卡尔曼滤波（HEKF）算法，其具体步骤如下，

目标状态的一步预测，

$\hat{X}(k+1|k)=f\left(\hat{X}\left(k\right|k)\right)---\left(17\right)$

目标状态一步预测对应的协方差矩阵为，

$P(k+1|k)=f_X\left(k\right)P\left(k\right|k)f_X^T\left(k\right)+Q\left(k\right)---\left(18\right)$

f_X(k)为函数f X k的Jacobian阵，

$f_X\left(k\right)\stackrel{\mathrm{\Δ}}{=}{\left[{\▿}_{X\left(k\right)}{f}^T\left(X\left(k\right)\right)\right]}^T---\left(19\right)$

为了得到量测的预测值，将式（14）中的非线性函数在

附近进行泰勒级数展开

$\begin{array}{c}\mathrm{Zk}+1=h\hat{X}k+1|k+h_X\hat{X}k+1|k[\mathrm{Xk}+1-\hat{X}k+1|k]+\\ \frac{1}{2}\underset{i=1}{\overset{n_x}{\mathrm{\Σ}}}{e}_i{[\mathrm{Xk}+1-\hat{X}k+1|k]}^T{h}_{\mathrm{XX}}^i\hat{X}k+1[\mathrm{Xk}+1-\hat{X}k+1|k]+\\ \·\·\·\end{array}---\left(20\right)$

其中，

为h的第i分量的海塞矩阵，n_x为状态向量的维数。

$h_{\mathrm{XX}}^i\stackrel{\mathrm{\Δ}}{=}{\left[{\▿}_X{\▿}_X^{\′}{h}^i\left(X(k+1)\right)\right]}_{X=\hat{X}(k+1|k)}---\left(21\right)$

通过对式（22）取数学期望，并略去二阶及以上的高阶项，得到从k到k+1时刻的量测预测值

$\hat{Z}(k+1|k)=h\left(\hat{X}(k+1|k)\right)=\left[\begin{array}{c}\sqrt{{\hat{x}}^2(k+1|k)+{\hat{y}}^2(k+1|k)}\\ \arctan [\hat{y}(k+1|k)/\hat{x}(k+1|k)]\end{array}\right]---\left(22\right)$

相应的量测的预测协方差（或新息协方差）为，

S(k+1)=h_X(X(k+1))P(k+1|k)h′_X(X(k+1))+R(k+1)   （23）

其中，h_X(X(k+1))为量测方程中的非线性函数在状态预测处进行一阶泰勒展开对应的雅可比矩阵，

$\begin{array}{c}{h}_X\left(X(k+1)\right)\stackrel{\mathrm{\Δ}}{=}{\left[{\▿}_{X}h\left(X(k+1)\right)\right]}_{X=\hat{X}(k+1|k)}\\ =\left[\begin{array}{cccc}\frac{\hat{x}(k+1|k)}{\hat{r}}& 0& \frac{\hat{y}(k+1|k)}{\hat{r}}& 0\\ -\frac{\hat{y}(k+1|k)}{{\hat{r}}^2}& 0& \frac{\hat{x}(k+1|k)}{{\hat{r}}^2}& 0\end{array}\right]\end{array}---\left(24\right)$

式中

$\hat{r}=\sqrt{{\hat{x}}^2(k+1|k)+{\hat{y}}^2(k+1|k)}---\left(25\right)$

HEKF的增益为，

$K(k+1)=P(k+1|k)h_X^T\left(X(k+1)\right)S^{-1}(k+1)---\left(26\right)$

从而，可求得k+1时刻的状态更新方程为，

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)[Z(k+1)-\hat{Z}(k+1|k)]---\left(27\right)$

协方差更新方程

P(k+1|k+1)＝[I-K(k+1)h_X(k+1)]P(k+1|k)[I+K(k+1)h_X(k+1)]^T

                                    （28）

-K(k+1)R(k+1)K^T(k+1)

其中，I是与状态协方差矩阵同维的单位矩阵。

当目标相对雷达距离很远时，坐标转换非线性程度高，使用一阶泰勒近似得到的关于量测的预测和新息矩阵的式（22）和式（23）会很不准确，因此可以使用更高量级的单位来表示目标相对雷达的距离，将实际中的“远距离”目标转换成数据处理中的“近距离”目标，降低了非线性函数的线性化误差，使得量测的预测和协方差估计的更为准确，从而产生了高量级扩展卡尔曼滤波算法（HEKF）。

本发明针对目标在二维平面内均匀直线运动且雷达观测在极坐标获得的场景，对远距离和近距离目标分别进行跟踪，将高量级扩展卡尔曼滤波（HEKF）与EKF和UKF进行对比，验证了HEKF的有效性。

Claims (1)

1.高量级扩展卡尔曼滤波方法，其特征在于，包括以下步骤：

第一步：假设目标在二维平面内运动，分别建立目标的状态方程和量测方程，如式(1)和式(2)所示；

X(k+1)＝F(k)X(k)+V(k)         (1)

$Z(k+1)=h\left(X(k+1)\right)+W(k+1)$

$=\left[\begin{array}{c}\sqrt{x^2(k+1)+y^2(k+1)}\\ \arctan [y(k+1)/x(k+1)]\end{array}\right]+W(k+1)$        (2)

其中X(k)是目标k时刻的状态向量，F(k)是状态转移矩阵，V(k)是目标运动的过程噪声，其协方差矩阵用Q(k)表示；Z(k+1)表示目标在k+1时刻的量测值，二维平面内包含两种量测量，即目标相对雷达的距离和方位角，分别用

和arctan[y(k+1)/x(k+1)]表示，x(k+1)和y(k+1)是k+1目标在直角坐标系下x轴和y轴上的位置分量，W(k+1)是k+1时刻雷达的量测误差；

第二步：根据经典一阶扩展卡尔曼滤波算法，由k时刻的滤波结果得到目标的k+1时刻目标状态预测方程

$\hat{X}(k+1|k)=F\left(k\right)\hat{X}\left(k\right|k)---\left(3\right)$

其中

表示k时刻的目标滤波向量，

中的元素采用高量级单位，在数值上变小；

第三步：计算预测协方差矩阵

P(k+1|k)＝F(k)P(k|k)F^T(k)+Q(k)         (4)

其中P(k|k)是扩展卡尔曼滤波器k时刻滤波误差的协方差矩阵；

第四步：计算扩展卡尔曼滤波器的增益矩阵

$K(k+1)=P(k+1|k)h_X^T(k+1){[h_X^T\left(k\right)P\left(k\right|k-1)h_X^T\left(k\right)+R\left(k\right)]}^{-1}---\left(5\right)$

其中，h_X(k+1)是雅克比矩阵，表示为

$h_X(k+1)={{\left[{\▿}_X{h}^{\′}(k+1,X)\right]}^{\′}}_{X=\hat{X}(k+1|k)}---\left(6\right)$

第五步：计算目标k+1的状态更新方程

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)[Z(k+1)-h[k+1,\hat{X}(k+1|k)\left]\right]---\left(7\right)$

量测值Z(k+1)使用高量级单位，数值上变小；

第六步：计算目标k+1时刻的协方差更新误差

P(k+1|k+1)＝P(k+1|k)-K(k+1)h_X(k+1)P(k+1|k)      (8)

第七步：根据目标在k+1时刻的状态滤波向量

和滤波误差的协方差矩阵P(k+1|k+1)，按第三步至第六步计算目标在k+2时刻的状态滤波向量

和滤波误差的协方差矩阵P(k+2|k+2)，以此循环往复，最终的输出滤波结果再换回原来的量级；

其中算子

的含义为：

$h_X(k+1)={{\left[{\▿}_X{h}^{\′}(k+1,X)\right]}^{\′}}_{X=\hat{X}(k+1|k)}$

$={\left[\begin{array}{cc}\left[\begin{array}{c}\∂/\∂x_1\\ .\\ .\\ .\\ \∂/\∂x_n\end{array}\right]& \left[\begin{array}{ccc}{h}_1\left(X\right)& ...& h_n\left(X\right)\end{array}\right]\end{array}\right]}_{X=\hat{X}(k+1|k)}^{\′}$

$={\left[\begin{array}{ccc}\frac{\∂h_1\left(X\right)}{\∂x_1}& ...& \frac{\∂h_n\left(X\right)}{\∂x_1}\\ .& & .\\ .& ...& .\\ .& & .\\ \frac{\∂h_1\left(X\right)}{\∂x_n}& ...& \frac{\∂h_n\left(X\right)}{\∂x_n}\end{array}\right]}_{X=\hat{X}(k+1|k)};$

算子R(k)的含义为量测噪声的协方差矩阵。