CN103278813A - 一种基于高阶无迹卡尔曼滤波的状态估计方法 - Google Patents

Abstract

一种基于高阶无迹卡尔曼滤波的状态估计方法，本发明具体涉及一种基于高阶无迹卡尔曼滤波的状态估计方法。本发明采用高阶无迹卡尔曼滤波器完成目标跟踪过程中的状态估计任务。在目标跟踪过程中，建立目标跟踪的状态方程和量测方程；采用高阶无迹变换获得目标跟踪滤波器所需的sigma点，并计算其权值；通过迭代sigma点及其权值获取对状态的估计，实现对目标的实时跟踪。其跟踪精度高于现有的基于其它滤波器的目标跟踪方法。通过选用合适的性能参数κ，能够进一步提升本发明提出的高阶UKF目标跟踪方法精度，实现对目标的高精度实时跟踪。本发明应用于目标跟踪技术领域。

Description

一种基于高阶无迹卡尔曼滤波的状态估计方法

技术领域

本发明属于非线性滤波及目标跟踪技术领域，具体涉及一种基于高阶无迹卡尔曼滤波的状态估计方法。

背景技术

在目标定位跟踪过程中，观测站通常仅能够获得包含噪声的目标方位信息，例如利用雷达对空中目标进行观测时，雷达能够获得空中目标相对自身的方位角，但该观测含有噪声。在假设目标是匀速飞行的情况下，需要估计目标位置从而实现对目标的跟踪。此时观测方程中雷达的方位观测量是待估计目标位置参数的非线性函数，不能直接利用线性滤波方法获得目标的运动状态，其本质为非线性滤波问题，是目标跟踪、机器视觉以及传感器网络定位等研究领域的共同难题。为了解决这一难题，人们首先试图使用线性化方法，从而产生了扩展卡尔曼滤波器(Extended Kalman Filter,EKF)。但是EKF在目标定位跟踪应用中存在两个很大的缺点，首先，当系统非线性很强时，围绕状态预测估值或状态估值的一阶泰勒展开方法往往存在很大的截断误差，使得跟踪效果不理想，甚至出现滤波发散的现象；其次一阶EKF需要计算Jacobi矩阵，二阶EKF还需要计算Hessian矩阵，计算量大，对处理器要求高。

为解决非线性滤波估计问题，研究人员提出一种新的用于解决非线性滤波问题的方法，它是基于这样一种考虑：近似一种高斯分布要比近似任何一种非线性函数容易的多。人们将这样一种滤波器称为无迹卡尔曼滤波器(Unscented Kalman Filter,UKF)。研究表明UKF给出的估计结果比EKF更准确，它能达到二阶EKF估计精度，但是计算量却远远小于二阶EKF。

目前人们在目标跟踪领域内应用的UKF都是二阶UKF方法，对于高斯非线性系统，二阶UKF的估计精度只能达到非线性函数的三次泰勒展开项，精度有限，对于一些对估计精度要求较高的场合，二阶UKF不再适用。

发明内容

本发明为了解决现有技术在目标定位跟踪的过程中，存在很大的截断误差，使得跟踪效果不理想，从而提出了一种基于高阶无迹卡尔曼滤波的状态估计方法。

一种基于高阶无迹卡尔曼滤波的状态估计方法包括下述步骤：

步骤一：建立描述目标跟踪系统的状态方程和量测方程；

步骤二：根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ；

步骤三：一步状态预测，通过高阶无迹变换获得用于一步状态预测的sigma点，并将所述的sigma点经状态方程传播得到一步状态预测的样本点，对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵；

步骤四：一步量测预测，通过高阶无迹变换获得用于一步量测预测的sigma点，并将所述的sigma点经量测方程传播得到一步量测预测的样本点，对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵，对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵；

步骤五：状态滤波更新，根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵，和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵，完成状态的滤波更新，得到状态的线性最小方差估计和估计误差协方差矩阵，完成目标跟踪的状态估计任务。

步骤一所述的建立描述目标跟踪系统的状态方程和量测方程为：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+n_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(1\right)$

其中，状态方程为x_k=f(x_k-1)+n_k-1，量测方程为z_k=h(x_k)+v_k，k-1表示第k-1步，k表示第k步，x_k为第k步的n维跟踪目标参数状态向量，z_k为第k+1步的m维跟踪目标参数的量测向量，f(·)和h(·)均为已知的非线性函数，n_k-1为第k-1步n维随机系统噪声，v_k为第k步m维随机量测噪声，随机系统噪声向量n_k-1服从均值为0、方差为Q_k-1的高斯分布，Q_k-1表示第k-1步系统噪声的方差阵，随机量测噪声向量v_k服从均值为0、方差为R_k的高斯分布，R_k表示第k步量测噪声的方差阵，且n_k-1与v_k不相关。

步骤二所述的根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ的具体过程为：

根据性能参数κ的一元二次方程：

(n-1)κ²+(2n²-14n)κ+n³-13n²+60n-60=0

求解性能参数κ，当系统的状态的维数n=2时，性能参数κ=0.835；

当系统的状态的维数n=3时，性能参数κ=1.417；

当系统的状态的维数n=1或n≥4时，性能参数κ的一元二次方程无实数解，即不存在理论最优的性能参数κ的值，设定κ=2。

步骤三所述的对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵的具体过程为：

第k-1步待估计跟踪目标参数构成的状态向量x_k-1的后验密度函数

k-1|k-1表示采用观测集合[z₀…z_k-1]对相应的参数进行估计，根据公式（2）分解误差协方差P_k-1|k-1获得矩阵S_k-1|k-1：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(2\right)$

根据公式（3）和（4）分别计算第一类sigma点X_00,k-1|k-1和该第一类sigma点的权值w₀：

$X_{00,k-1|k-1}={\hat{x}}_{k-1|k-1}---\left(3\right)$

$w_0=\frac{-2n^2+(4-2n){\mathrm{\κ}}^2+(4\mathrm{\κ}+4)n}{{(n+\mathrm{\κ})}^2(4-n)}---\left(4\right)$

根据公式（5）和（6）计算具有相同权值的2n个第二类sigma点

和其权值w₁：

$\left\{\begin{array}{c}{X}_{{1i}_1,k-1|k-1}=\sqrt{\frac{(4-n)(n+\mathrm{\κ})}{(\mathrm{\κ}+2-n)}}{S}_{k-1|k-1}{e}_{i_1}+{\hat{x}}_{k-1|k-1}\\ X_{{2i}_1,k-1|k-1}=-\sqrt{\frac{(4-n)(n+\mathrm{\κ})}{(\mathrm{\κ}+2-n)}}{S}_{k-1|k-1}{e}_{i_1}+{\hat{x}}_{k-1|k-1}\end{array}\right.---\left(5\right)$

$w_1=\frac{{(\mathrm{\κ}+2-n)}^2}{2{(n+\mathrm{\κ})}^2(4-n)}---\left(6\right)$

其中，i₁=1,2,...,n，

表示第i₁个坐标为1的单位列向量，

根据公式（7）和（8）计算具有相同权值的2n(n-1)个第三类sigma点

和该第三类sigma点的权值w₂：

$\left\{\begin{array}{c}{X}_{{3i}_2,k-1|k-1}=\sqrt{(n+\mathrm{\κ})}{S}_{k-1|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k-1|k-1}\\ X_{{4i}_2,k-1|k-1}=-\sqrt{(n+\mathrm{\κ})}{S}_{k-1|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k-1|k-1}\\ X_{{5i}_2,k-1|k-1}=\sqrt{(n+\mathrm{\κ})}{S}_{k-1|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k-1|k-1}\\ X_{{6i}_2,k-1|k-1}=-\sqrt{(n+\mathrm{\κ})}{S}_{k-1|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k-1|k-1}\end{array}\right.---\left(7\right)$

$w_2=\frac{1}{{(n+\mathrm{\κ})}^2}---\left(8\right)$

其中，i₂=1,2,...,n(n-1)/2，

$\left\{s_{i_2}^{+}\right\}\stackrel{\mathrm{\&Delta;}}{=}\{\sqrt{\frac{1}{2}}(e_p+e_l):p<l,p,l=\mathrm{1,2},.....n\}---\left(9\right)$

$\left\{s_{i_2}^{-}\right\}\stackrel{\mathrm{\&Delta;}}{=}\{\sqrt{\frac{1}{2}}(e_p-e_l):p<l,p,l=\mathrm{1,2},.....n\}---\left(10\right)$

其中，e_l=[0,0,....1,...0]^T表示第l个坐标为1的单位列向量；e_p=[0,0,....1,...0]^T表示第p个坐标为1的单位列向量，

根据公式（11）计算状态方程传播sigma点：

$\left\{\begin{array}{c}{X}_{00,k|k-1}^{*}=f\left(X_{00,k-1|k-1}\right)\\ X_{{1i}_1,k|k-1}^{*}=f\left(X_{{1i}_1,k-1|k-1}\right)\\ X_{{2i}_1,k|k-1}^{*}=f\left(X_{{2i}_1,k-1|k-1}\right)\\ X_{{3i}_2,k|k-1}^{*}=f\left(X_{{3i}_2,k-1|k-1}\right)\\ X_{{4i}_2,k|k-1}^{*}=f\left(X_{{4i}_2,k-1|k-1}\right)\\ X_{{5i}_2,k|k-1}^{*}=f\left(X_{{5i}_2,k-1|k-1}\right)\\ X_{{6i}_2,k|k-1}^{*}=f\left(X_{{6i}_2,k-1|k-1}\right)\end{array}\right.---\left(11\right)$

根据公式（12）估计第k步的一步状态预测估计

${\hat{x}}_{k|k-1}=w_0{X}_{00,k|k-1}^{*}+w_1\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}^{*}+X_{{2i}_1,k|k-1}^{*})$ $\left(12\right)$ $+w_2\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}^{*}+X_{{4i}_2,k|k-1}^{*}+X_{{5i}_2,k|k-1}^{*}+X_{{6i}_2,k|k-1}^{*})$

根据公式（13）估计第k步的一步状态预测估计误差协方差矩阵P_k|k-1：

$P_{k|k-1}=w_0{X}_{00,k|k-1}^{*}{X}_{00,k|k-1}^{*T}+w_1\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}^{*}{X}_{{1i}_1,k|k-1}^{*T}+X_{{2i}_1,k|k-1}^{*}{X}_{{2i}_1,k|k-1}^{*T})$ $+w_2\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}^{*}{X}_{{3i}_2,k|k-1}^{*T}+X_{{4i}_2,k|k-1}^{*}{X}_{{4i}_2,k|k-1}^{*T}+X_{{5i}_2,k|k-1}^{*}{X}_{{5i}_2,k|k-1}^{*T}+X_{{6i}_2,k|k-1}^{*}{X}_{{6i}_2,k|k-1}^{*T})$ $-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}---\left(13\right).$

步骤四所述的对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵，对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵的具体步骤为：

根据公式（14）分解一步状态预测估计误差协方差矩阵P_k|k-1获得S_k|k-1：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T---\left(14\right)$

根据公式（15）和（16）计算第一类sigma点X_00,k|k-1和该第一类sigma点X_00,k|k-1的权值w₁₀：

$X_{00,k|k-1}={\hat{x}}_{k|k-1}---\left(15\right)$

$w_{10}=\frac{-2n^2+(4-2n){\mathrm{\&kappa;}}^2+(4\mathrm{\&kappa;}+4)n}{{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(16\right)$

根据公式（17）和（18）计算具有相同权值的2n个第二类sigma点

和该第二类sigma点的权值w₁₁：

$\left\{\begin{array}{c}{X}_{{1i}_1,k|k-1}=\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k|k-1}{e}_{i_1}+{\hat{x}}_{k|k-1}\\ X_{{2i}_1,k|k-1}=-\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k|k-1}{e}_{i_1}+{\hat{x}}_{k|k-1}\end{array}\right.---\left(17\right)$

$w_{11}=\frac{{(\mathrm{\&kappa;}+2-n)}^2}{2{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(18\right)$

根据公式（19）和（20）计算具有相同权值的2n(n-1)个第三类sigma点

和该第三类sigma点的权值w₁₂：

$\left\{\begin{array}{c}{X}_{{3i}_2,k|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k|k-1}\\ X_{{4i}_2,k|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k|k-1}\\ X_{{5i}_2,k|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k|k-1}\\ X_{{6i}_2,k|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k|k-1}\end{array}\right.---\left(19\right)$

$w_{12}=\frac{1}{{(n+\mathrm{\&kappa;})}^2}---\left(20\right)$

根据公式（21）计算量测方程传播sigma点：

$\left\{\begin{array}{c}{Z}_{00,k|k-1}=h\left(X_{00,k|k-1}\right)\\ Z_{{1i}_1,k|k-1}=h\left(X_{{1i}_1,k|k-1}\right)\\ Z_{{2i}_1,k|k-1}=h\left(X_{{2i}_1,k|k-1}\right)\\ Z_{{3i}_2,k|k-1}=h\left(X_{{3i}_2,k|k-1}\right)\\ Z_{{4i}_2,k|k-1}=h\left(X_{{4i}_2,k|k-1}\right)\\ Z_{{5i}_2,k|k-1}=h\left(X_{{5i}_2,k|k-1}\right)\\ Z_{{6i}_2,k|k-1}=h\left(X_{{6i}_2,k|k-1}\right)\end{array}\right.---\left(21\right)$

根据公式（22）估计第k步的量测的一步量测预测估计

${\hat{z}}_{k|k-1}=w_{10}{Z}_{00,k|k-1}+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(Z_{{1i}_1,k|k-1}+Z_{{2i}_1,k|k-1})$ $\left(22\right)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(Z_{{3i}_2,k|k-1}+Z_{{4i}_2,k|k-1}+Z_{{5i}_2,k|k-1}+Z_{{6i}_2,k|k-1})$

根据公式（23）估计第k步量测的一步估计误差协方差矩阵P_zz,k|k-1：

$P_{\mathrm{zz},k|k-1}=w_{10}{Z}_{00,k|k-1}{Z}_{00,k|k-1}^T+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(Z_{{1i}_1,k|k-1}{Z}_{{1i}_1,k|k-1}^T+Z_{{2i}_1,k|k-1}{Z}_{{2i}_1,k|k-1}^T)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(Z_{{3i}_2,k|k-1}{Z}_{{3i}_2,k|k-1}^T+Z_{{4i}_2,k|k-1}{Z}_{{4i}_2,k|k-1}^T+Z_{{5i}_2,k|k-1}{Z}_{{5i}_2,k|k-1}^T+Z_{{6i}_2,k|k-1}{Z}_{{6i}_2,k|k-1}^T)$ $-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k---\left(23\right)$

根据公式（24）估计状态与量测的互协方差矩阵：

$P_{\mathrm{xz},k|k-1}=w_{10}{X}_{00,k|k-1}{Z}_{00,k|k-1}^T+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}{Z}_{{1i}_1,k|k-1}^T+X_{{2i}_1,k|k-1}{Z}_{{2i}_1,k|k-1}^T)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}{Z}_{{3i}_2,k|k-1}^T+X_{{4i}_2,k|k-1}{Z}_{{4i}_2,k|k-1}^T+X_{{5i}_2,k|k-1}{Z}_{{5i}_2,k|k-1}^T+X_{{6i}_2,k|k-1}{Z}_{{6i}_2,k|k-1}^T)-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T---\left(24\right).$

步骤五所述的根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵，和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵，完成状态的滤波更新，得到状态的线性最小方差估计和估计误差协方差矩阵的具体过程为：

根据公式（25）估计高阶UKF的滤波增益W_k：

$W_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1}---\left(25\right)$

根据公式（26）计算第k步状态滤波估计

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+W_k(z_k-{\hat{z}}_{k|k-1})---\left(26\right)$

根据公式（27）计算第k步状态估计误差协方差矩阵P_k|k计算如下：

$P_{k|k}=P_{k|k-1}-W_k{P}_{\mathrm{zz},k|k-1}{W}_k^T---\left(27\right).$

本发明的优点在于：

1、通过建立目标跟踪的数学模型，并引入一个性能参数κ给出了一种基于高阶UKF的目标跟踪方法。实现了截断误差小的目的。

2、通过理论分析和实际运行经验，得到在二维和三维情况下，当κ分别取κ=0.835和κ=1.417时，高阶UKF可以获得比现有的二阶UKF、二阶CKF及高阶CKF方法更优的性能，达到了提高目标跟踪的精度的目的。

附图说明

图1是本发明所述的一种基于高阶无迹卡尔曼滤波的状态估计方法程图；

图2为图1的详细方法流程图；

图3是本发明提供的方法与基于二阶UKF、二阶CKF及高阶CKF的目标跟踪应用中对目标x坐标轴方向位置估计的均方误差曲线；图中A表示二阶CKF对目标x坐标轴方向位置估计的均方误差曲线，B表示二阶UKF对目标x坐标轴方向位置估计的均方误差曲线，C表示高阶CKF对目标x坐标轴方向位置估计的均方误差曲线，D表示本发明提供的方法对目标x坐标轴方向位置估计的均方误差曲线；

图4是本发明提供的方法与基于二阶UKF、二阶CKF及高阶CKF的目标跟踪应用中对目标y坐标轴方向位置估计的均方误差曲线。

具体实施方式

具体实施方式一、结合图1具体说明本实施方式，本实施方式所述的一种基于高阶无迹卡尔曼滤波的状态估计方法包括下述步骤：

步骤一：建立描述目标跟踪系统的状态方程和量测方程；

步骤二：根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ；

步骤三：一步状态预测，通过高阶无迹变换(Unscented Transformation，UT)获得用于一步状态预测的sigma点，并将所述的sigma点经状态方程传播得到一步状态预测的样本点，对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵；

步骤四：一步量测预测，通过高阶无迹变换获得用于一步量测预测的sigma点，并将所述的sigma点经量测方程传播得到一步量测预测的样本点，对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵，对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵；

步骤五：状态滤波更新，根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵，和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵，完成状态的滤波更新，得到状态的线性最小方差估计和估计误差协方差矩阵，完成目标跟踪的状态估计任务。

具体实施方式二、本实施方式与具体实施方式一所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于，步骤一所述的建立描述目标跟踪系统的状态方程和量测方程为：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+n_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(1\right)$

其中，状态方程为x_k=f(x_k-1)+n_k-1，量测方程为z_k=h(x_k)+v_k，k-1表示第k-1步，k表示第k步，x_k为第k步的n维跟踪目标参数状态向量，z_k为第k+1步的m维跟踪目标参数的量测向量，f(·)和h(·)均为已知的非线性函数，n_k-1为第k-1步n维随机系统噪声，v_k为第k步m维随机量测噪声，随机系统噪声向量n_k-1服从均值为0、方差为Q_k-1的高斯分布，Q_k-1表示第k-1步系统噪声的方差阵，随机量测噪声向量v_k服从均值为0，方差为R_k的高斯分布，R_k表示第k步量测噪声的方差阵，且n_k-1与v_k不相关。

具体实施方式三、结合图2具体说明本实施方式，本实施方式与具体实施方式一或二所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于，步骤二所述的根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ的具体过程为：

获取性能参数κ的最优值时，sigma点必须能捕获到先验随机向量4阶矩信息从而得到性能参数κ的一元二次方程：

(n-1)κ²+(2n²-14n)κ+n³-13n²+60n-60=0

求解性能参数κ，当系统的状态的维数n=2时，性能参数κ=0.835；当系统的状态的维数n=3时，性能参数κ=1.417；此时本发明提供的方法可以获得比现有的二阶UKF、二阶CKF及高阶CKF方法更优的性能；

当系统的状态的维数n=1或n≥4时，性能参数κ的一元二次方程无实数解，即不存在理论最优的性能参数κ的值，根据无迹变换的数值稳定角度考虑，设定κ=2，以获得跟踪稳定性。

具体实施方式四、本实施方式与具体实施方式三所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于，步骤三所述的对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵的具体过程为：

第k-1步待估计跟踪目标参数构成的状态向量x_k-1的后验密度函数

k-1|k-1表示采用观测集合[z₀…z_k-1]对相应的参数进行估计，根据公式（2）分解误差协方差P_k-1|k-1获得矩阵S_k-1|k-1：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(2\right)$

根据公式（3）和（4）分别计算第一类sigma点X_00,k-1|k-1和该第一类sigma点的权值w₀：

$X_{00,k-1|k-1}={\hat{x}}_{k-1|k-1}---\left(3\right)$

$w_0=\frac{-2n^2+(4-2n){\mathrm{\&kappa;}}^2+(4\mathrm{\&kappa;}+4)n}{{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(4\right)$

根据公式（5）和（6）计算具有相同权值的2n个第二类sigma点

和其权值w₁：

$\left\{\begin{array}{c}{X}_{{1i}_1,k-1|k-1}=\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k-1|k-1}{e}_{i_1}+{\hat{x}}_{k-1|k-1}\\ X_{{2i}_1,k-1|k-1}=-\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k-1|k-1}{e}_{i_1}+{\hat{x}}_{k-1|k-1}\end{array}\right.---\left(5\right)$

$w_1=\frac{{(\mathrm{\&kappa;}+2-n)}^2}{2{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(6\right)$

其中，i₁=1,2,...,n，表示第i₁个坐标为1的单位列向量，

根据公式（7）和（8）计算具有相同权值的2n(n-1)个第三类sigma点

和该第三类sigma点的权值w₂：

$\left\{\begin{array}{c}{X}_{{3i}_2,k-1|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k-1|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k-1|k-1}\\ X_{{4i}_2,k-1|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k-1|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k-1|k-1}\\ X_{{5i}_2,k-1|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k-1|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k-1|k-1}\\ X_{{6i}_2,k-1|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k-1|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k-1|k-1}\end{array}\right.---\left(7\right)$

$w_2=\frac{1}{{(n+\mathrm{\&kappa;})}^2}---\left(8\right)$

其中，i₂=1,2,...,n(n-1)/2，

$\left\{s_{i_2}^{+}\right\}\stackrel{\mathrm{\&Delta;}}{=}\{\sqrt{\frac{1}{2}}(e_p+e_l):p<l,p,l=\mathrm{1,2},.....n\}---\left(9\right)$

$\left\{s_{i_2}^{-}\right\}\stackrel{\mathrm{\&Delta;}}{=}\{\sqrt{\frac{1}{2}}(e_p-e_l):p<l,p,l=\mathrm{1,2},.....n\}---\left(10\right)$

其中，e_l=[0,0,....1,...0]^T表示第l个坐标为1的单位列向量；e_p=[0,0,....1,...0]^T表示第p个坐标为1的单位列向量，

根据公式（11）计算状态方程传播sigma点：

$\left\{\begin{array}{c}{X}_{00,k|k-1}^{*}=f\left(X_{00,k-1|k-1}\right)\\ X_{{1i}_1,k|k-1}^{*}=f\left(X_{{1i}_1,k-1|k-1}\right)\\ X_{{2i}_1,k|k-1}^{*}=f\left(X_{{2i}_1,k-1|k-1}\right)\\ X_{{3i}_2,k|k-1}^{*}=f\left(X_{{3i}_2,k-1|k-1}\right)\\ X_{{4i}_2,k|k-1}^{*}=f\left(X_{{4i}_2,k-1|k-1}\right)\\ X_{{5i}_2,k|k-1}^{*}=f\left(X_{{5i}_2,k-1|k-1}\right)\\ X_{{6i}_2,k|k-1}^{*}=f\left(X_{{6i}_2,k-1|k-1}\right)\end{array}\right.---\left(11\right)$

根据公式（12）估计第k步的一步状态预测估计

${\hat{x}}_{k|k-1}=w_0{X}_{00,k|k-1}^{*}+w_1\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}^{*}+X_{{2i}_1,k|k-1}^{*})$ $\left(12\right)$ $+w_2\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}^{*}+X_{{4i}_2,k|k-1}^{*}+X_{{5i}_2,k|k-1}^{*}+X_{{6i}_2,k|k-1}^{*})$

根据公式（13）估计第k步的一步状态预测估计误差协方差矩阵P_k|k-1：

$P_{k|k-1}=w_0{X}_{00,k|k-1}^{*}{X}_{00,k|k-1}^{*T}+w_1\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}^{*}{X}_{{1i}_1,k|k-1}^{*T}+X_{{2i}_1,k|k-1}^{*}{X}_{{2i}_1,k|k-1}^{*T})$ $+w_2\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}^{*}{X}_{{3i}_2,k|k-1}^{*T}+X_{{4i}_2,k|k-1}^{*}{X}_{{4i}_2,k|k-1}^{*T}+X_{{5i}_2,k|k-1}^{*}{X}_{{5i}_2,k|k-1}^{*T}+X_{{6i}_2,k|k-1}^{*}{X}_{{6i}_2,k|k-1}^{*T})$ $-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}---\left(13\right).$

具体实施方式五、本实施方式与具体实施方式四所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于，步骤四所述的对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵，对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵的具体步骤为：

根据公式（14）分解一步状态预测估计误差协方差矩阵P_k|k-1获得S_k|k-1：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T---\left(14\right)$

根据公式（15）和（16）计算第一类sigma点X_00,k|k-1和该第一类sigma点X_00,k|k-1的权值w₁₀：

$X_{00,k|k-1}={\hat{x}}_{k|k-1}---\left(15\right)$

$w_{10}=\frac{-2n^2+(4-2n){\mathrm{\&kappa;}}^2+(4\mathrm{\&kappa;}+4)n}{{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(16\right)$

根据公式（17）和（18）计算具有相同权值的2n个第二类sigma点和该第二类sigma点的权值w₁₁：

$\left\{\begin{array}{c}{X}_{{1i}_1,k|k-1}=\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k|k-1}{e}_{i_1}+{\hat{x}}_{k|k-1}\\ X_{{2i}_1,k|k-1}=-\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k|k-1}{e}_{i_1}+{\hat{x}}_{k|k-1}\end{array}\right.---\left(17\right)$

$w_{11}=\frac{{(\mathrm{\&kappa;}+2-n)}^2}{2{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(18\right)$

根据公式（19）和（20）计算具有相同权值的2n(n-1)个第三类sigma点

和该第三类sigma点的权值w₁₂：

$\left\{\begin{array}{c}{X}_{{3i}_2,k|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k|k-1}\\ X_{{4i}_2,k|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k|k-1}\\ X_{{5i}_2,k|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k|k-1}\\ X_{{6i}_2,k|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k|k-1}\end{array}\right.---\left(19\right)$

$w_{12}=\frac{1}{{(n+\mathrm{\&kappa;})}^2}---\left(20\right)$

根据公式（21）计算量测方程传播sigma点：

$\left\{\begin{array}{c}{Z}_{00,k|k-1}=h\left(X_{00,k|k-1}\right)\\ Z_{{1i}_1,k|k-1}=h\left(X_{{1i}_1,k|k-1}\right)\\ Z_{{2i}_1,k|k-1}=h\left(X_{{2i}_1,k|k-1}\right)\\ Z_{{3i}_2,k|k-1}=h\left(X_{{3i}_2,k|k-1}\right)\\ Z_{{4i}_2,k|k-1}=h\left(X_{{4i}_2,k|k-1}\right)\\ Z_{{5i}_2,k|k-1}=h\left(X_{{5i}_2,k|k-1}\right)\\ Z_{{6i}_2,k|k-1}=h\left(X_{{6i}_2,k|k-1}\right)\end{array}\right.---\left(21\right)$

根据公式（22）估计第k步的量测的一步量测预测估计

${\hat{z}}_{k|k-1}=w_{10}{Z}_{00,k|k-1}+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(Z_{{1i}_1,k|k-1}+Z_{{2i}_1,k|k-1})$ $\left(22\right)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(Z_{{3i}_2,k|k-1}+Z_{{4i}_2,k|k-1}+Z_{{5i}_2,k|k-1}+Z_{{6i}_2,k|k-1})$

根据公式（23）估计第k步量测的一步估计误差协方差矩阵P_zz,k|k-1：

$P_{\mathrm{zz},k|k-1}=w_{10}{Z}_{00,k|k-1}{Z}_{00,k|k-1}^T+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(Z_{{1i}_1,k|k-1}{Z}_{{1i}_1,k|k-1}^T+Z_{{2i}_1,k|k-1}{Z}_{{2i}_1,k|k-1}^T)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(Z_{{3i}_2,k|k-1}{Z}_{{3i}_2,k|k-1}^T+Z_{{4i}_2,k|k-1}{Z}_{{4i}_2,k|k-1}^T+Z_{{5i}_2,k|k-1}{Z}_{{5i}_2,k|k-1}^T+Z_{{6i}_2,k|k-1}{Z}_{{6i}_2,k|k-1}^T)$ $-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k---\left(23\right)$

根据公式（24）估计状态与量测的互协方差矩阵：

$P_{\mathrm{xz},k|k-1}=w_{10}{X}_{00,k|k-1}{Z}_{00,k|k-1}^T+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}{Z}_{{1i}_1,k|k-1}^T+X_{{2i}_1,k|k-1}{Z}_{{2i}_1,k|k-1}^T)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}{Z}_{{3i}_2,k|k-1}^T+X_{{4i}_2,k|k-1}{Z}_{{4i}_2,k|k-1}^T+X_{{5i}_2,k|k-1}{Z}_{{5i}_2,k|k-1}^T+X_{{6i}_2,k|k-1}{Z}_{{6i}_2,k|k-1}^T)-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T---\left(24\right).$

具体实施方式六、本实施方式与具体实施方式五所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于，步骤五所述的根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵，和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵，完成状态的滤波更新，得到状态的线性最小方差估计和估计误差协方差矩阵的具体过程为：

根据公式（25）估计高阶UKF的滤波增益W_k：

$W_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1}---\left(25\right)$

根据公式（26）计算第k步状态滤波估计

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+W_k(z_k-{\hat{z}}_{k|k-1})---\left(26\right)$

根据公式（27）计算第k步状态估计误差协方差矩阵P_k|k计算如下：

$P_{k|k}=P_{k|k-1}-W_k{P}_{\mathrm{zz},k|k-1}{W}_k^T---\left(27\right).$

此时通过式(26)可以获得的第k步状态滤波估计，实现对目标的跟踪。式(26)给出的第k步状态滤波估计和式(27)给出的第k步状态估计误差协方差矩阵P_k|k将用于下一时刻目标位置参数的估计。

实施例：在目标跟踪过程中，观测站能够获得包含噪声的目标方位信息，而方位信息与待估计的目标位置信息间关系为非线性函数，造成了观测方程的非线性，通常应用EKF、二阶UKF、二阶CKF或高阶CKF非线性滤波方法来获得目标的运动状态。但在对目标定位跟踪精度要求较高的场合，上述滤波方法不能满足要求。本发明提供的方法比现有的方法具备更高的估计精度，能够提高目标跟踪的精度。下面以具体实施例子来说明本发明的优越性。具体如下：

步骤一：依据纯方位目标跟踪问题，建立如下描述目标跟踪系统的状态方程和观测方程：

$x_k=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]x_{k-1}+n_{k-1}---\left(28\right)$

$z_x={\tan }^{-1}\left(\frac{x_{2,k}-\sin \left(k\right)}{x_{1,k}-\cos \left(k\right)}\right)+v_k---\left(29\right)$

其中，第k步的状态方程x_k=[x_1,k x_2,k]^T=[x y]^T，表示x轴和y轴平面内（笛卡尔坐标系）目标的位置； $f\left(x_{k-1}\right)=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]x_{k-1},$ 矩阵 $\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]$ 表征了目标x方向速度为0.9/秒，y方向速度为1米/秒。随机系统噪声向量n_k-1服从均值为0，方差为Q_k-1的高斯分布， $Q_{k-1}=\left[\begin{array}{cc}{0.1m}^2& {0.05m}^2\\ {0.05m}^2& {0.1m}^2\end{array}\right];$ 随机量测噪声向量v_k服从均值为0，方差为R_k的高斯分布，R_k=0.025rad²，初始真实状态值x₀=[20m5m]^T，初始协方差阵P₀=[0.1m²0m²;0m²0.1m²]^T，P₀表征了目标初始位置的不确定性。

步骤二：根据系统状态的维数选择合适的高阶UKF性能参数κ，纯方位跟踪系统状态维数为n=2，因此取最优的κ值，κ=0.835。

步骤三：第k步的一步状态预测，应用高阶无迹变换获得用于状态一步预测的sigma点，并将这些sigma点经状态方程传播得到一步状态预测的样本点，加权计算得到的一步状态预测估计和一步状态预测估计误差协方差矩阵。

具体为，假设k-1步后验密度函数

已知，通过Cholesky分解，根据公式（2）误差协方差P_k-1|k-1，

根据根据公式（3）和（4）分别计算第一类sigma点X_00,k-1|k-1和该第一类sigma点的权值w₀，得到：

w₀＝0.416

根据公式（5）和（6）计算具有相同权值的2n个第二类sigma点

和其权值w₁，其中i₁=1,2

获得：

$\left\{\begin{array}{c}{X}_{{1i}_1,k-1|k-1}={2.606S}_{k-1|k-1}{e}_{i_1}+{\hat{x}}_{k-1|k-1}\\ X_{{2i}_1,k-1|k-1}=-2.606S_{k-1|k-1}{e}_{i_1}+{\hat{x}}_{k-1|k-1}\end{array}\right.$

w₁=0.022

根据公式（7）和（8）计算具有相同权值的2n(n-1)个第三类sigma点

和该第三类sigma点的权值w₂，

获得：

$\left\{\begin{array}{c}{X}_{{3i}_2,k-1|k-1}=1.684S_{k-1|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k-1|k-1}\\ X_{{4i}_2,k-1|k-1}=-1.684S_{k-1|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k-1|k-1}\\ X_{{5i}_2,k-1|k-1}=1.684S_{k-1|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k-1|k-1}\\ X_{{6i}_2,k-1|k-1}=-1.684S_{k-1|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k-1|k-1}\end{array}\right.$

w₂=0.124

其中，i₂=1，在公式（9）和（10）中p<l，l，p=1,2

根据公式（11）计算状态方程传播sigma点，得到：

$X_{00,k|k-1}^{*}=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]X_{00,k-1|k-1}$

$X_{{1i}_1,k|k-1}^{*}=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]X_{{1i}_1,k-1|k-1}$

$X_{{2i}_1,k|k-1}^{*}=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]X_{{2i}_1,k-1|k-1}$

$X_{{3i}_2,k|k-1}^{*}=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]X_{{3i}_2,k-1|k-1}$

$X_{{4i}_2,k|k-1}^{*}=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]X_{{4i}_2,k-1|k-1}$

$X_{{5i}_2,k|k-1}^{*}=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]X_{{5i}_2,k-1|k-1}$

$X_{{6i}_2,k|k-1}^{*}=\left[\begin{array}{cc}0.9& 0\\ 0& 1\end{array}\right]X_{{6i}_2,k-1|k-1}$

根据公式（12）估计第k步的一步状态预测估计

得到：

${\hat{x}}_{k|k-1}=0.416X_{00,k|k-1}^{*}+0.022\underset{i_1=1}{\overset{2}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}^{*}+X_{{2i}_1,k|k-1}^{*})$ $+0.124(X_{{3i}_2,k|k-1}^{*}+X_{{4i}_2,k|k-1}^{*}+X_{{5i}_2,k|k-1}^{*}+X_{{6i}_2,k|k-1}^{*})$

根据公式（13）估计第k步的一步状态预测估计误差协方差矩阵P_k|k-1，得到：

$P_{k|k-1}=0.416X_{00,k|k-1}^{*}{X}_{00,k|k-1}^{*T}+0.022\underset{i_1=1}{\overset{2}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}^{*}{X}_{{1i}_1,k|k-1}^{*T}+X_{{2i}_1,k|k-1}^{*}{X}_{{2i}_1,k|k-1}^{*T})$ $+0.124(X_{{3i}_2,k|k-1}^{*}{X}_{{3i}_2,k|k-1}^{*T}+X_{{4i}_2,k|k-1}^{*}{X}_{{4i}_2,k|k-1}^{*T}+X_{{5i}_2,k|k-1}^{*}{X}_{{5i}_2,k|k-1}^{*T}+X_{{6i}_2,k|k-1}^{*}{X}_{{6i}_2,k|k-1}^{*T})$ $-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+\left[\begin{array}{cc}0.1& 0.05\\ 0.05& 0.1\end{array}\right]$

步骤四：一步量测预测，应用高阶UT变换获得用于量测一步预测的sigma点，并将这些sigma点经量测方程传播得到量测一步预测的样本点，加权计算得到量测的一步预测估计和一步估计误差协方差矩阵，以及状态与量测的互协方差矩阵。

具体为，根据公式（14）分解一步状态预测估计误差协方差矩阵P_k|k-1获得S_k|k-1，根据公式（15）和（16）计算第一类sigma点X_00,k|k-1和该第一类sigma点X_00,k|k-1的权值w₁₀，获得：

$X_{00,k|k-1}={\hat{x}}_{k|k-1}$

w₁₀=0.416

根据公式（17）和（18）计算具有相同权值的2n个第二类sigma点

和该第二类sigma点的权值w₁₁，获得：

$\left\{\begin{array}{c}{X}_{{1i}_1,k|k-1}={2.606S}_{k|k-1}{e}_{i_1}+{\hat{x}}_{k|k-1}\\ X_{{2i}_1,k|k-1}=-2.606S_{k|k-1}{e}_{i_1}+{\hat{x}}_{k|k-1}\end{array}\right.$

w₁₁=0.022

根据公式（19）和（20）计算具有相同权值的2n(n-1)个第三类sigma点

和该第三类sigma点的权值w₁₂，获得：

$\left\{\begin{array}{c}{X}_{{3i}_2,k|k-1}=1.684S_{k|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k|k-1}\\ X_{{4i}_2,k|k-1}=-1.684S_{k|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k|k-1}\\ X_{{3i}_2,k|k-1}=1.684S_{k|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k|k-1}\\ X_{{4i}_2,k|k-1}=-1.684S_{k|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k|k-1}\end{array}\right.$

w₁₂=0.124

根据公式（21）计算量测方程传播sigma点，得到：

$Z_{00,k|k-1}={\tan }^{-1}\left(\frac{X_{00,k|k-1}\left(2\right)-\sin \left(k\right)}{X_{00,k|k-1}\left(1\right)-\cos \left(k\right)}\right)$

$Z_{{1i}_1,k|k-1}={\tan }^{-1}\left(\frac{X_{{1i}_1,k|k-1}\left(2\right)-\sin \left(k\right)}{X_{{1i}_1,k|k-1}\left(1\right)-\cos \left(k\right)}\right)$

$Z_{{2i}_1,k|k-1}={\tan }^{-1}\left(\frac{X_{{2i}_1,k|k-1}\left(2\right)-\sin \left(k\right)}{X_{{2i}_1,k|k-1}\left(1\right)-\cos \left(k\right)}\right)$

$Z_{{3i}_2,k|k-1}={\tan }^{-1}\left(\frac{X_{{3i}_2,k|k-1}\left(2\right)-\sin \left(k\right)}{X_{{3i}_2,k|k-1}\left(1\right)-\cos \left(k\right)}\right)$

$Z_{{4i}_2,k|k-1}={\tan }^{-1}\left(\frac{X_{{4i}_2,k|k-1}\left(2\right)-\sin \left(k\right)}{X_{{4i}_2,k|k-1}\left(1\right)-\cos \left(k\right)}\right)$

$Z_{{5i}_2,k|k-1}={\tan }^{-1}\left(\frac{X_{{5i}_2,k|k-1}\left(2\right)-\sin \left(k\right)}{X_{{5i}_2,k|k-1}\left(1\right)-\cos \left(k\right)}\right)$

$Z_{{6i}_2,k|k-1}={\tan }^{-1}\left(\frac{X_{{6i}_2,k|k-1}\left(2\right)-\sin \left(k\right)}{X_{{6i}_2,k|k-1}\left(1\right)-\cos \left(k\right)}\right)$

根据公式（22）估计第k步的量测的一步量测预测估计

获得：

${\hat{z}}_{k|k-1}=0.416Z_{0i,k|k-1}+0.022\underset{i_1=1}{\overset{2}{\mathrm{\&Sigma;}}}(Z_{{1i}_1,k|k-1}+Z_{{2i}_1,k|k-1})$ $+0.124(Z_{{3i}_2,k|k-1}+Z_{{4i}_2,k|k-1}+Z_{{5i}_2,k|k-1}+Z_{{6i}_2,k|k-1})$

根据公式（23）估计第k步量测的一步估计误差协方差矩阵P_zz,k|k-1，获得：

$P_{\mathrm{zz},k|k-1}=0.416Z_{00,k|k-1}{Z}_{00,k|k-1}^T+0.022\underset{j=1}{\overset{2}{\mathrm{\&Sigma;}}}(Z_{{1i}_1,k|k-1}{Z}_{{1i}_1,k|k-1}^T+Z_{{2i}_1,k|k-1}{Z}_{{2i}_1,k|k-1}^T)$ $+0.124(Z_{{3i}_2,k|k-1}{Z}_{{3i}_2,k|k-1}^T+Z_{{4i}_2,k|k-1}{Z}_{{4i}_2,k|k-1}^T+Z_{{5i}_2,k|k-1}{Z}_{{5i}_2,k|k-1}^T+Z_{{6i}_2,k|k-1}{Z}_{{6i}_2,k|k-1}^T)$ $-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+0.025$

根据公式（24）估计状态与量测的互协方差矩阵，获得：

$P_{\mathrm{xz},k|k-1}=0.416X_{0i,k|k-1}{Z}_{00,k|k-1}^T+0.022\underset{i_1=1}{\overset{2}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}{Z}_{{1i}_1,k|k-1}^T+X_{{2i}_1,k|k-1}{Z}_{{2i}_1,k|k-1}^T)$ $+0.124(X_{{3i}_2,k|k-1}{Z}_{{3i}_2,k|k-1}^T+X_{{4i}_2,k|k-1}{Z}_{{4i}_2,k|k-1}^T+X_{{5i}_2,k|k-1}{Z}_{{5i}_2,k|k-1}^T+X_{{6i}_2,k|k-1}{Z}_{{6i}_2,k|k-1}^T)-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$

步骤五：状态滤波更新，应用前面步骤得到的状态和量测一步预测参数，完成状态的滤波更新，得到状态的线性最小方差估计和估计误差协方差矩阵，完成纯方位跟踪的目标位置估计任务。

具体为，根据公式（25）估计高阶UKF的滤波增益W_k，根据公式（26）计算第k步状态滤波估计根据公式（27）计算第k步状态估计误差协方差矩阵P_k|k，

此时，

即为目标的位置估计，P_k|k为目标的位置估计误差协方差矩阵。

和P_k|k将用于下一时刻对目标的位置和位置误差协方差矩阵的计算中。

实施过程：仿真过程中采用如下定义的均方误差性能指标，比较各种滤波方法：

$\mathrm{MSE}\left(k\right)=\frac{1}{N}\underset{n=1}{\overset{N}{\mathrm{\&Sigma;}}}{(x_{i,k}^n-x_{i,k/k}^n)}^2(i=\mathrm{1,2})---\left(30\right)$

其中N为Monte Carlo次数。对目标位置估计的均方误差值越小则表征目标跟踪精度越高，效果越好。

仿真时间100秒，在相同的仿真条件下采用二阶UKF、二阶CKF、高阶CKF和本发明提出的高阶UKF方法，在本发明方法中，参数κ取0.835。所有的方法均进行500次MonteCarlo仿真，以进行比较。

实施效果：图3给出了纯方位目标跟踪过程中，本发明提供的方法与二阶UKF、二阶CKF、高阶CKF方法对x方向位置估计的均方误差曲线；图4给出了纯方位目标跟踪过程中，本发明提供的方法与二阶UKF、二阶CKF、高阶CKF对y方向位置估计的均方误差曲线。在图3和图4中，曲线A代表二阶UKF状态估计值的均方误差曲线，曲线B代表二阶CKF状态估计值的均方误差曲线，曲线C代表高阶CKF状态估计值的均方误差曲线，曲线D代表本发明提供方法状态估计值的均方误差曲线，状态估计值的均方误差越小代表估计精度越高，性能越好。从图3和图4可以看出，二阶UKF性能优于二阶CKF，高阶CKF性能优于二阶UKF，所有的滤波方法中本发明提出的高阶UKF（κ=0.835）具备最优的性能。

从以上实施例不难看出，相对于现有的目标跟踪方法，本发明提供方法能够获得更高的精度，获取对目标更准确的跟踪。

Claims (6)

1.一种基于高阶无迹卡尔曼滤波的状态估计方法，其特征在于：它包括下述步骤：

步骤一：建立描述目标跟踪系统的状态方程和量测方程；

步骤二：根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ；

步骤三：一步状态预测，通过高阶无迹变换获得用于一步状态预测的sigma点，并将所述的sigma点经状态方程传播得到一步状态预测的样本点，对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵；

步骤四：一步量测预测，通过高阶无迹变换获得用于一步量测预测的sigma点，并将所述的sigma点经量测方程传播得到一步量测预测的样本点，对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵，对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵；

步骤五：状态滤波更新，根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵，和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵，完成状态的滤波更新，得到状态的线性最小方差估计和估计误差协方差矩阵，完成目标跟踪的状态估计任务。

2.根据权利要求1所述的一种基于高阶无迹卡尔曼滤波的状态估计方法，其特征在于：步骤一所述的建立描述目标跟踪系统的状态方程和量测方程为：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+n_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(1\right)$

其中，状态方程为x_k=f(x_k-1)+n_k-1，量测方程为z_k=h(x_k)+v_k，k-1表示第k-1步，k表示第k步，x_k为第k步的n维跟踪目标参数状态向量，z_k为第k+1步的m维跟踪目标参数的量测向量，f(·)和h(·)均为已知的非线性函数，n_k-1为第k-1步n维随机系统噪声，v_k为第k步m维随机量测噪声，随机系统噪声向量n_k-1服从均值为0，方差为Q_k-1的高斯分布，Q_k-1表示第k-1步系统噪声的方差阵，随机量测噪声向量v_k服从均值为0、方差为R_k的高斯分布，R_k表示第k步量测噪声的方差阵，且n_k-1与v_k不相关。

3.根据权利要求1或2所述的一种基于高阶无迹卡尔曼滤波的状态估计方法，其特征在于：步骤二所述的根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ的具体过程为：

根据性能参数κ的一元二次方程：

(n-1)κ²+(2n²-14n)κ+n³-13n²+60n-60=0

求解性能参数κ，当系统的状态的维数n=2时，性能参数κ=0.835；

当系统的状态的维数n=3时，性能参数κ=1.417；

当系统的状态的维数n=1或n≥4时，性能参数κ的一元二次方程无实数解，即不存在理论最优的性能参数κ的值，设定κ=2。

4.根据权利要求3所述的一种基于高阶无迹卡尔曼滤波的状态估计方法，其特征在于：步骤三所述的对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵的具体过程为：

第k-1步待估计跟踪目标参数构成的状态向量x_k-1的后验密度函数k-1|k-1表示采用观测集合[z₀…z_k-1]对相应的参数进行估计，根据公式（2）分解误差协方差P_k-1|k-1获得矩阵S_k-1|k-1：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T---\left(2\right)$

根据公式（3）和（4）分别计算第一类sigma点X_00,k-1|k-1和该第一类sigma点的权值w₀：

$X_{00,k-1|k-1}={\hat{x}}_{k-1|k-1}---\left(3\right)$

$w_0=\frac{-2n^2+(4-2n){\mathrm{\&kappa;}}^2+(4\mathrm{\&kappa;}+4)n}{{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(4\right)$

根据公式（5）和（6）计算具有相同权值的2n个第二类sigma点和其权值w₁：

$\left\{\begin{array}{c}{X}_{{1i}_1,k-1|k-1}=\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k-1|k-1}{e}_{i_1}+{\hat{x}}_{k-1|k-1}\\ X_{{2i}_1,k-1|k-1}=-\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k-1|k-1}{e}_{i_1}+{\hat{x}}_{k-1|k-1}\end{array}\right.---\left(5\right)$

$w_1=\frac{{(\mathrm{\&kappa;}+2-n)}^2}{2{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(6\right)$

其中，i₁=1,2,...,n，

表示第i₁个坐标为1的单位列向量，根据公式（7）和（8）计算具有相同权值的2n(n-1)个第三类sigma点

和该第三类sigma点的权值w₂：

$\left\{\begin{array}{c}{X}_{{3i}_2,k-1|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k-1|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k-1|k-1}\\ X_{{4i}_2,k-1|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k-1|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k-1|k-1}\\ X_{{5i}_2,k-1|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k-1|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k-1|k-1}\\ X_{{6i}_2,k-1|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k-1|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k-1|k-1}\end{array}\right.---\left(7\right)$

$w_2=\frac{1}{{(n+\mathrm{\&kappa;})}^2}---\left(8\right)$

其中，i₂=1,2,...,n(n-1)/2，

$\left\{s_{i_2}^{+}\right\}\stackrel{\mathrm{\&Delta;}}{=}\{\sqrt{\frac{1}{2}}(e_p+e_l):p<l,p,l=\mathrm{1,2},.....n\}---\left(9\right)$

$\left\{s_{i_2}^{-}\right\}\stackrel{\mathrm{\&Delta;}}{=}\{\sqrt{\frac{1}{2}}(e_p-e_l):p<l,p,l=\mathrm{1,2},.....n\}---\left(10\right)$

其中，e_l=[0,0,....1,...0]^T表示第l个坐标为1的单位列向量；e_p=[0,0,....1,...0]^T表示第p个坐标为1的单位列向量，

根据公式（11）计算状态方程传播sigma点：

$\left\{\begin{array}{c}{X}_{00,k|k-1}^{*}=f\left(X_{00,k-1|k-1}\right)\\ X_{{1i}_1,k|k-1}^{*}=f\left(X_{{1i}_1,k-1|k-1}\right)\\ X_{{2i}_1,k|k-1}^{*}=f\left(X_{{2i}_1,k-1|k-1}\right)\\ X_{{3i}_2,k|k-1}^{*}=f\left(X_{{3i}_2,k-1|k-1}\right)\\ X_{{4i}_2,k|k-1}^{*}=f\left(X_{{4i}_2,k-1|k-1}\right)\\ X_{{5i}_2,k|k-1}^{*}=f\left(X_{{5i}_2,k-1|k-1}\right)\\ X_{{6i}_2,k|k-1}^{*}=f\left(X_{{6i}_2,k-1|k-1}\right)\end{array}\right.---\left(11\right)$

根据公式（12）估计第k步的一步状态预测估计

${\hat{x}}_{k|k-1}=w_0{X}_{00,k|k-1}^{*}+w_1\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}^{*}+X_{{2i}_1,k|k-1}^{*})$ $\left(12\right)$ $+w_2\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}^{*}+X_{{4i}_2,k|k-1}^{*}+X_{{5i}_2,k|k-1}^{*}+X_{{6i}_2,k|k-1}^{*})$

根据公式（13）估计第k步的一步状态预测估计误差协方差矩阵P_k|k-1：

$P_{k|k-1}=w_0{X}_{00,k|k-1}^{*}{X}_{00,k|k-1}^{*T}+w_1\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}^{*}{X}_{{1i}_1,k|k-1}^{*T}+X_{{2i}_1,k|k-1}^{*}{X}_{{2i}_1,k|k-1}^{*T})$ $+w_2\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}^{*}{X}_{{3i}_2,k|k-1}^{*T}+X_{{4i}_2,k|k-1}^{*}{X}_{{4i}_2,k|k-1}^{*T}+X_{{5i}_2,k|k-1}^{*}{X}_{{5i}_2,k|k-1}^{*T}+X_{{6i}_2,k|k-1}^{*}{X}_{{6i}_2,k|k-1}^{*T})$

$-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}---\left(13\right).$

5.根据权利要求4所述的一种基于高阶无迹卡尔曼滤波的状态估计方法，其特征在于：步骤四所述的对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵，对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵的具体步骤为：

根据公式（14）分解一步状态预测估计误差协方差矩阵P_k|k-1获得S_k|k-1：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T---\left(14\right)$

根据公式（15）和（16）计算第一类sigma点X_00,k|k-1和该第一类sigma点X_00,k|k-1的权值w₁₀：

$X_{00,k|k-1}={\hat{x}}_{k|k-1}---\left(15\right)$

$w_{10}=\frac{-2n^2+(4-2n){\mathrm{\&kappa;}}^2+(4\mathrm{\&kappa;}+4)n}{{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(16\right)$

根据公式（17）和（18）计算具有相同权值的2n个第二类sigma点和该第二类sigma点的权值w₁₁：

$\left\{\begin{array}{c}{X}_{{1i}_1,k|k-1}=\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k|k-1}{e}_{i_1}+{\hat{x}}_{k|k-1}\\ X_{{2i}_1,k|k-1}=-\sqrt{\frac{(4-n)(n+\mathrm{\&kappa;})}{(\mathrm{\&kappa;}+2-n)}}{S}_{k|k-1}{e}_{i_1}+{\hat{x}}_{k|k-1}\end{array}\right.---\left(17\right)$

$w_{11}=\frac{{(\mathrm{\&kappa;}+2-n)}^2}{2{(n+\mathrm{\&kappa;})}^2(4-n)}---\left(18\right)$

根据公式（19）和（20）计算具有相同权值的2n(n-1)个第三类sigma点

和该第三类sigma点的权值w₁₂：

$\left\{\begin{array}{c}{X}_{{3i}_2,k|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k|k-1}\\ X_{{4i}_2,k|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{+}+{\hat{x}}_{k|k-1}\\ X_{{5i}_2,k|k-1}=\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k|k-1}\\ X_{{6i}_2,k|k-1}=-\sqrt{(n+\mathrm{\&kappa;})}{S}_{k|k-1}{s}_{i_2}^{-}+{\hat{x}}_{k|k-1}\end{array}\right.---\left(19\right)$

$w_{12}=\frac{1}{{(n+\mathrm{\&kappa;})}^2}---\left(20\right)$

根据公式（21）计算量测方程传播sigma点：

$\left\{\begin{array}{c}{Z}_{00,k|k-1}=h\left(X_{00,k|k-1}\right)\\ Z_{{1i}_1,k|k-1}=h\left(X_{{1i}_1,k|k-1}\right)\\ Z_{{2i}_1,k|k-1}=h\left(X_{{2i}_1,k|k-1}\right)\\ Z_{{3i}_2,k|k-1}=h\left(X_{{3i}_2,k|k-1}\right)\\ Z_{{4i}_2,k|k-1}=h\left(X_{{4i}_2,k|k-1}\right)\\ Z_{{5i}_2,k|k-1}=h\left(X_{{5i}_2,k|k-1}\right)\\ Z_{{6i}_2,k|k-1}=h\left(X_{{6i}_2,k|k-1}\right)\end{array}\right.---\left(21\right)$

根据公式（22）估计第k步的量测的一步量测预测估计

${\hat{z}}_{k|k-1}=w_{10}{Z}_{00,k|k-1}+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(Z_{{1i}_1,k|k-1}+Z_{{2i}_1,k|k-1})$ $\left(22\right)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(Z_{{3i}_2,k|k-1}+Z_{{4i}_2,k|k-1}+Z_{{5i}_2,k|k-1}+Z_{{6i}_2,k|k-1})$

根据公式（23）估计第k步量测的一步估计误差协方差矩阵P_zz,k|k-1：

$P_{\mathrm{zz},k|k-1}=w_{10}{Z}_{00,k|k-1}{Z}_{00,k|k-1}^T+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(Z_{{1i}_1,k|k-1}{Z}_{{1i}_1,k|k-1}^T+Z_{{2i}_1,k|k-1}{Z}_{{2i}_1,k|k-1}^T)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(Z_{{3i}_2,k|k-1}{Z}_{{3i}_2,k|k-1}^T+Z_{{4i}_2,k|k-1}{Z}_{{4i}_2,k|k-1}^T+Z_{{5i}_2,k|k-1}{Z}_{{5i}_2,k|k-1}^T+Z_{{6i}_2,k|k-1}{Z}_{{6i}_2,k|k-1}^T)$ $-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k---\left(23\right)$

根据公式（24）估计状态与量测的互协方差矩阵：

$P_{\mathrm{xz},k|k-1}=w_{10}{X}_{00,k|k-1}{Z}_{00,k|k-1}^T+w_{11}\underset{i_1=1}{\overset{n}{\mathrm{\&Sigma;}}}(X_{{1i}_1,k|k-1}{Z}_{{1i}_1,k|k-1}^T+X_{{2i}_1,k|k-1}{Z}_{{2i}_1,k|k-1}^T)$ $+w_{12}\underset{i_2=1}{\overset{n(n-1)/2}{\mathrm{\&Sigma;}}}(X_{{3i}_2,k|k-1}{Z}_{{3i}_2,k|k-1}^T+X_{{4i}_2,k|k-1}{Z}_{{4i}_2,k|k-1}^T+X_{{5i}_2,k|k-1}{Z}_{{5i}_2,k|k-1}^T+X_{{6i}_2,k|k-1}{Z}_{{6i}_2,k|k-1}^T)-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T---\left(24\right).$

6.根据权利要求5所述的一种基于高阶无迹卡尔曼滤波的状态估计方法，其特征在于：步骤五所述的根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵，和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵，完成状态的滤波更新，得到状态的线性最小方差估计和估计误差协方差矩阵的具体过程为：

根据公式（25）估计高阶UKF的滤波增益W_k：

$W_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1}---\left(25\right)$

根据公式（26）计算第k步状态滤波估计

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+W_k(z_k-{\hat{z}}_{k|k-1})---\left(26\right)$

根据公式（27）计算第k步状态估计误差协方差矩阵P_k|k：

$P_{k|k}=P_{k|k-1}-W_k{P}_{\mathrm{zz},k|k-1}{W}_k^T---\left(27\right).$

Images