CN104931932A - 一种改进的去偏坐标转换卡尔曼滤波方法 - Google Patents

Abstract

为解决目标跟踪方法中跟踪性能随着方位角、俯仰角和观测距离增大而增大的问题，本发明提出一种改进的去偏坐标转换卡尔曼滤波方法。本发明采用乘性无偏转换方法对量测值进行转换，通过卡尔曼滤波预测值实现转换测量均方差估计，再引入无迹变换方法得到转换测量均方差值，最后通过卡尔曼滤波算法实现雷达对目标的精确跟踪。

Description

一种改进的去偏坐标转换卡尔曼滤波方法

技术领域

本发明涉及一种无线传感器网络领域和空间多目标跟踪领域，具体来说是一种改进的去偏坐标转换卡尔曼滤波方法。

背景技术

随着信息时代的发展，航天技术不断的创新提高，空间在各国的政治、经济、军事等领域内的战略地位日益提高。在现代化的军事斗争中，空间信息俨然已经成为了最核心的战斗能力，对于空间信息的掌握至关重要。为了更好的掌握空间信息，同时随着卫星技术和雷达技术的发展，研究出了新型的天基雷达对空间目标进行探测和跟踪，为了更加精确的识别、跟踪空间目标信息，在跟踪雷达中必须引入目标跟踪方法，利用目标的观测信息来更新目标的状态信息，提高天基雷达对空间目标的跟踪精度。

目标跟踪方法是雷达数据处理的重要环节，在雷达对航天器、空间碎片、卫星、不明星球等目标进行探测跟踪时，目标跟踪方法扮演着越来越重要的角色，利用观测到的数据对运动目标的状态进行预测，从而预测下一时刻的目标的运动位置，以完成目标运动轨迹的跟踪。同时，目标跟踪技术在民用方面也应用广泛，如空中交通管制，机器人技术等，目前它已成为非常活跃的研究领域之一。二十世纪中期，卡尔曼滤波方法的出现，大力推动了目标跟踪技术的发展。目前常用的目标跟踪方法有：线性滤波方法主要有卡尔曼滤波方法以及由卡尔曼滤波为基础推导出的常增益滤波方法；为满足更高的跟踪精度需求，跟踪滤波方法由线性滤波方法发展到非线性滤波方法，常用的非线性的滤波方法有扩展卡尔曼滤波方法、无迹卡尔曼滤波方法与粒子滤波方法。

在1993年D.Lerro等人提出另外一种区别于UT变换的线性化方法，即利用坐标转换方法来实现线性化，提出了二维空间中的去偏混合坐标系转换卡尔曼滤波方法，后来有杨春玲等人提出了三维空间中的去偏混合坐标系转换卡尔曼滤波方法。但是去偏坐标转换卡尔曼滤波方法的跟踪性能会随着方位角和俯仰角的标准差、观测距离的增大而增大。本发明就能很好的解决这个问题，其跟踪性能更加稳定。

发明内容

本发明为了解决了上述难题，提出一种改进的去偏坐标转换卡尔曼滤波方法，本方法通过改进二维空间的基于卡尔曼滤波预测的无偏量测转换方法，提出三维空间的基于卡尔曼滤波预测的无偏量测转换方法。

一种改进的去偏坐标转换卡尔曼滤波方法，具体步骤如下：

步骤1、建立系统模型，设定本方法的目标的状态方程和测量方程；

步骤2、对目标初始状态和误差协方差进行初始化；

步骤3、在k-1时刻，通过系统模型对k时刻目标的状态和误差协方差进行预测；

步骤4、通过新的观测值对测量方程进行更新；

步骤5、对测量误差的均方误差进行更新；

步骤6、使用卡尔曼标准滤波方法进行滤波估计。

下面具体阐述改进的去偏坐标转换卡尔曼滤波方法过程。本发明在极坐标系下通过观测得到目标的观测值，采用乘性无偏转换法对观测值进行坐标转换，并在极坐标系下利用卡尔曼滤波方法对转换测量误差均方差进行预测估计，并通过无迹变换计算获得，最后在直角坐标系下进行卡尔曼滤波，实现雷达对目标的精确跟踪。

x、y、z分别为目标在直角坐标系中的真实位置，r、β、θ为目标在极坐标下的真实位置。观测平台在极坐标系下得到的测量值分别为：r_m、β_m、θ_m，设在极坐标系下真实值和测量值之间的误差为：测量距离误差为俯仰角误差为方位角误差为的均方差分别为：σ_r、σ_β、σ_θ，它们是相互独立的，并且均值为0。用公式表示为：

$\begin{array}{c}{r}_m=r+{\tilde{r}}_m\\ {\mathrm{\β}}_m=\mathrm{\β}+{\widetilde{\mathrm{\β}}}_m\\ {\mathrm{\θ}}_m=\mathrm{\θ}+{\widetilde{\mathrm{\θ}}}_m\end{array}---\left(1\right)$

根据直角坐标和极坐标的转换关系可得到：

x_m＝r_mcosβ_mcosθ_m

y_m＝r_mcosβ_msinθ_m    (2)

z_m＝r_msinβ_m

(2)式中的x_m、y_m、z_m为坐标转换得到的X、Y、Z轴上的测量值。把(1)式带入(2)式得到：

$\begin{array}{c}{x}_m=(r+{\tilde{r}}_m)\cos (\mathrm{\β}+{\widetilde{\mathrm{\β}}}_m)\cos (\mathrm{\θ}+{\widetilde{\mathrm{\θ}}}_m)\\ y_m=(r+{\tilde{r}}_m)\cos (\mathrm{\β}+{\widetilde{\mathrm{\β}}}_m)\sin (\mathrm{\θ}+{\widetilde{\mathrm{\θ}}}_m)\\ z_m=(r+{\tilde{r}}_m)\sin (\mathrm{\β}+{\widetilde{\mathrm{\β}}}_m)\end{array}---\left(3\right)$

设为存在雷达直角坐标系下的转换测量误差，有：

$\begin{array}{c}{x}_m=x+{\tilde{x}}_m\\ y_m=y+{\tilde{y}}_m\\ z_m=z+{\tilde{z}}_m\end{array}---\left(4\right)$

因此，可以表示为：

$\begin{array}{c}{\tilde{x}}_m=x_m-x=(r+{\tilde{r}}_m)\cos (\mathrm{\β}+{\widetilde{\mathrm{\β}}}_m)\cos (\mathrm{\θ}+{\widetilde{\mathrm{\θ}}}_m)-r\cos \mathrm{\β}\cos \mathrm{\θ}\\ {\tilde{y}}_m=y_m-y=(r+{\tilde{r}}_m)\cos (\mathrm{\β}+{\widetilde{\mathrm{\β}}}_m)\sin (\mathrm{\θ}+{\widetilde{\mathrm{\θ}}}_m)-r\cos \mathrm{\β}\sin \mathrm{\θ}\\ {\tilde{z}}_m=z_m-z=(r+{\tilde{r}}_m)\sin (\mathrm{\β}+{\widetilde{\mathrm{\β}}}_m)-r\sin \mathrm{\β}\end{array}---\left(5\right)$

本发明采用乘性无偏转换方法对量测值进行转换满足：

E[x_m|r,θ,β]-x＝0

E[y_m|r,θ,β]-y＝0    (6)

E[z_m|r,θ,β]-z＝0

假设在极坐标系下利用卡尔曼滤波预测得到的目标的预测距离为r_p、方位角为θ_p、俯仰角为β_p，满足关系式为：

$\begin{array}{c}{r}_p=r+{\tilde{r}}_p\\ {\mathrm{\θ}}_p=\mathrm{\θ}+{\widetilde{\mathrm{\θ}}}_p\\ {\mathrm{\β}}_p=\mathrm{\β}+{\widetilde{\mathrm{\β}}}_p\end{array}---\left(7\right)$

其中，令为东北极坐标下的相互独立的噪声，并且标准差分别为

在东北极坐标下的卡尔曼滤波预测值(r_p,θ_p,β_p)得转换测量均方差为：

$R=\left[\begin{array}{ccc}{R}^x& R^{\mathrm{xy}}& R^{\mathrm{xz}}\\ R^{\mathrm{xy}}& R^y& R^{\mathrm{yz}}\\ R^{\mathrm{xz}}& R^{\mathrm{yz}}& R^z\end{array}\right]---\left(8\right)$

把(3)式带入(8)式可以得到R的元素分别为：

$\begin{array}{c}{R}^x=\frac{1}{4}E\left(r_m^2\right)\·\{1+E\left(\cos \left(2\mathrm{\β}\right)\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\β}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\β}}}_m\right)\left]\right\}\·\{1+E\left(\cos \left(2\mathrm{\θ}\right)\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\θ}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\θ}}}_m\right)\left]\right\}\\ -\frac{1}{4}E\left(r^2\right)\·[1+E\left(\cos \left(2\mathrm{\β}\right)\right)]\·[1+E\left(\cos \left(2\mathrm{\θ}\right)\right)]\end{array}---\left(9\right)$

$\begin{array}{c}{R}^y=\frac{1}{4}E\left(r_m^2\right)\·\{1+E\left(\cos \left(2\mathrm{\β}\right)\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\β}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\β}}}_m\right)\left]\right\}\·\{1-E\left(\cos \left(2\mathrm{\θ}\right)\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\θ}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\θ}}}_m\right)\left]\right\}\\ -\frac{1}{4}E\left(r^2\right)\·[1+E\left(\cos \left(2\mathrm{\β}\right)\right)]\·[1-E\left(\cos \left(2\mathrm{\θ}\right)\right)]\end{array}---\left(10\right)$

$R^z=\frac{1}{2}E\left(r_m^2\right)\·\{1-E\left(\cos \left(2\mathrm{\β}\right)\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\β}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\β}}}_m\right)\left]\right\}-\frac{1}{2}E\left(r^2\right)[1-E\left(\cos \left(2\mathrm{\β}\right)\right)]---\left(11\right)$

$\begin{array}{c}{R}^{\mathrm{xy}}=\frac{1}{4}E\left(r_m^2\right)\·\{1+E\left(\cos \left(2\mathrm{\β}\right)\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\β}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\β}}}_m\right)\left]\right\}\·E\left(\sin \left(2\mathrm{\θ}\right)\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\θ}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\θ}}}_m\right)]\\ -\frac{1}{4}E\left(r^2\right)\·[1+E\left(\cos \left(2\mathrm{\β}\right)\right)]\·E\left(\sin \left(2\mathrm{\θ}\right)\right)\end{array}---\left(12\right)$

$R^{\mathrm{xz}}=\frac{1}{2}E\left(\sin \left(2\mathrm{\β}\right)\right)E\left(\cos \mathrm{\θ}\right)\·\{E\left(r_m^2\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\β}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\β}}}_m\right)]\·E\left(\cos {\widetilde{\mathrm{\θ}}}_m\right)-E\left(r^2\right)\}---\left(13\right)$

$R^{\mathrm{yz}}=\frac{1}{2}[E\left(r_m^2\right)\·[E\left({\cos }^2{\widetilde{\mathrm{\β}}}_m\right)-E\left({\sin }^2{\widetilde{\mathrm{\β}}}_m\right)]\·E\left(\cos {\widetilde{\mathrm{\θ}}}_m\right)-E\left(r^2\right)]\·E\left(\sin \left(2\mathrm{\β}\right)\right)E\left(\sin \mathrm{\θ}\right)---\left(14\right)$

设极坐标下的测量误差为相互独立、零均值的高斯白噪声。存在如下关系：

$E\left(r_m^2\right)=r_p^2+{\mathrm{\σ}}_r^2+{\mathrm{\σ}}_{r_p}^2-{2r}_{p}E\left({\tilde{r}}_p\right)+{\left[E\left({\tilde{r}}_p\right)\right]}^2;E\left(r^2\right)=r_p^2+{\mathrm{\σ}}_{r_p}^2-{2r}_{p}E\left({\tilde{r}}_p\right)+{\left[E\left({\tilde{r}}_p\right)\right]}^2---\left(15\right)$

$E\left[\cos {\widetilde{\mathrm{\β}}}_m\right]=e^{-{\mathrm{\σ}}_{\mathrm{\β}}^2/2};E\left[\sin {\widetilde{\mathrm{\β}}}_m\right]=0;E\left[\cos {\widetilde{\mathrm{\θ}}}_m\right]=e^{-{\mathrm{\σ}}_{\mathrm{\θ}}^2/2};E\left[\sin {\widetilde{\mathrm{\θ}}}_m\right]=0---\left(16\right)$

$E\left[{\cos }^2{\widetilde{\mathrm{\β}}}_m\right]=1/2(1+e^{-2{\mathrm{\σ}}_{\mathrm{\β}}^2});E\left[{\sin }^2{\widetilde{\mathrm{\β}}}_m\right]=1/2(1-e^{-2{\mathrm{\σ}}_{\mathrm{\β}}^2})---\left(17\right)$

$E\left[{\cos }^2{\widetilde{\mathrm{\θ}}}_m\right]=1/2(1+e^{-2{\mathrm{\σ}}_{\mathrm{\θ}}^2});E\left[{\sin }^2{\widetilde{\mathrm{\θ}}}_m\right]=1/2(1-e^{-2{\mathrm{\σ}}_{\mathrm{\θ}}^2})---\left(18\right)$

本发明采用无迹变换方法计算获得R的值。

测量误差均值的表达式为：

$E\left[{\mathrm{\μ}}_t(r,\mathrm{\β},\mathrm{\θ})\right|r_m,{\mathrm{\β}}_m,{\mathrm{\θ}}_m]\stackrel{\mathrm{\Δ}}{=}{\mathrm{\μ}}_m---\left(19\right)$

根据(5)式可以推导出在知道测量值情况下的测量误差均值计算公式为：

${\mathrm{\μ}}_m=\left[\begin{array}{c}E\left[\tilde{x}\right|r_m,{\mathrm{\β}}_m,{\mathrm{\θ}}_m]\\ E\left[\tilde{y}\right|r_m,{\mathrm{\β}}_m,{\mathrm{\θ}}_m]\\ E\left[\tilde{z}\right|r_m,{\mathrm{\β}}_m,{\mathrm{\θ}}_m]\end{array}\right]=\left[\begin{array}{c}{r}_m\cos {\mathrm{\β}}_m\cos {\mathrm{\θ}}_m(e^{-{\mathrm{\σ}}_{\mathrm{\θ}}^2-{\mathrm{\σ}}_{\mathrm{\β}}^2}-e^{(-{\mathrm{\σ}}_{\mathrm{\θ}}^2/2-{\mathrm{\σ}}_{\mathrm{\β}}^2/2)})\\ r_m\cos {\mathrm{\β}}_m\sin {\mathrm{\θ}}_m(e^{-{\mathrm{\σ}}_{\mathrm{\θ}}^2-{\mathrm{\σ}}_{\mathrm{\β}}^2}-e^{(-{\mathrm{\σ}}_{\mathrm{\θ}}^2/2-{\mathrm{\σ}}_{\mathrm{\β}}^2/2)})\\ r_m\sin {\mathrm{\β}}_m(e^{-{\mathrm{\σ}}_{\mathrm{\β}}^2}-e^{(-{\mathrm{\σ}}_{\mathrm{\β}}^2/2)})\end{array}\right]---\left(20\right)$

可以使用去偏测量值来更新滤波测量值，见公式(21)：

$z_m^d=\left[\begin{array}{c}{x}_m\\ y_m\\ z_m\end{array}\right]-{\mathrm{\μ}}_m=\left[\begin{array}{c}{r}_m\cos {\mathrm{\β}}_m\cos {\mathrm{\θ}}_m\\ r_m\cos {\mathrm{\β}}_m\sin {\mathrm{\θ}}_m\\ r_m\sin {\mathrm{\β}}_m\end{array}\right]-{\mathrm{\μ}}_m---\left(21\right)$

把(20)式带入(21)式得到：

$z_m^d=\left[\begin{array}{c}({1-e}^{-{\mathrm{\σ}}_{\mathrm{\θ}}^2-{\mathrm{\σ}}_{\mathrm{\β}}^2}+e^{({-\mathrm{\σ}}_{\mathrm{\θ}}^2/2-{\mathrm{\σ}}_{\mathrm{\β}}^2/2)})r_m\cos {\mathrm{\β}}_m\cos {\mathrm{\θ}}_m\\ (1-e^{-{\mathrm{\σ}}_{\mathrm{\θ}}^2-{\mathrm{\σ}}_{\mathrm{\β}}^2}+e^{({-\mathrm{\σ}}_{\mathrm{\θ}}^2/2-{\mathrm{\σ}}_{\mathrm{\β}}^2/2)})r_m\cos {\mathrm{\β}}_m\sin {\mathrm{\θ}}_m\\ ({1-e}^{{-\mathrm{\σ}}_{\mathrm{\β}}^2}-e^{({-\mathrm{\σ}}_{\mathrm{\β}}^2/2)})r_m\sin {\mathrm{\β}}_m\end{array}\right]---\left(22\right)$

本发明的目标状态方程为：

X_k＝ΦX_k-1+Γv_k-1(k≥1)    (23)

其中，X_k＝[x,v_x,y,v_y,z,v_z]^T为目标状态向量，Φ∈R^n×n为状态转移矩阵，Γ∈R^n×p为过程噪声分布矩阵，v_k∈R^p×1为过程噪声。设在系统中的过程噪声v_k为零均值、高斯白噪声序列，其协方差为Q_k。

测量方程为：

z_k＝HX_k+w_k    (24)

其中，H为测量矩阵，w_k为测量噪声。再利用上面章节所讲的去偏坐标转换方法，代替z_k，得到：

$z_{m,k}^d=\left[\begin{array}{c}{x}_{m,k}\\ y_{m,k}\\ z_{m,k}\end{array}\right]-{\mathrm{\μ}}_{m,k}=H{X}_k+w_k---\left(25\right)$

最后通过标准卡尔曼滤波计算得到滤波增益K_k，滤波误差协方差P_k和滤波状态估计

附图说明

图1为本发明流程图；

图2为DCMKF和IDCMKF的位置均方根误差；

图3为DCMKF和IDCMKF的速度均方根误差；

图4为DCMKF和IDCMKF的位置均方根误差(增大目标位置)；

图5为DCMKF和IDCMKF的速度均方根误差(增大目标位置)。

具体实施方式

为使本发明的上述特征和优点能更加明显易懂，下面结合图1和具体实施方式对本发明作进一步详细说明。

首先将引入涉及到本方案的简写注释如下：

DCMKF为传统的去偏坐标转换卡尔曼滤波；

IDCMKF为改进的去偏坐标转换卡尔曼滤波(即本发明)。

其次，将引入涉及到本方案的相关参数，并详细描述如下：

x、y、z分别为目标在直角坐标系中的真实位置；

r、β、θ为目标在极坐标下的真实位置；

r_m、β_m、θ_m分别为观测平台在极坐标系下得到的测量值(距离、俯仰角和方位角)；

为极坐标系下真实值和测量值之间测量距离误差；

为极坐标系下真实值和测量值之间测量俯仰角误差；

为极坐标系下真实值和测量值之间测量方位角误差；

σ_r、σ_β、σ_θ分别为的均方差；

x_m、y_m、z_m分别为坐标转换得到的X、Y、Z轴上的测量值；

分别为存在直角坐标系下的转换测量误差；

r_p、θ_p、β_p为极坐标系下利用卡尔曼滤波预测得到的目标的预测距离、方位角、俯仰角；

分别为极坐标下的相互独立的噪声，并且标准差分别为

x_p、y_p、z_p分别为知己哦啊坐标系下卡尔曼滤波预测得到的目标的预测位置；

μ_m、R分别为测量误差均值和均方差矩阵；

X_k＝[x,v_x,y,v_y,z,v_z]^T为目标状态向量；

Φ∈R^n×n为状态转移矩阵；

Γ∈R^n×p为过程噪声分布矩阵；

v_k∈R^p×1为过程噪声，Q_k为其协方差；

z_k为测量值；

为去偏测量值；

H为测量转换矩阵；

w_k为测量噪声；

K为基于去偏坐标转换的α-β滤波算法的滤波增益；

P_k为k时刻滤波误差协方差；

q_x、q_y、q_z分别为X,Y,Z轴上的过程噪声；

N为仿真帧数；

M为蒙特卡罗采样次数；

T为采样周期；

为滤波估计。

本发明具体实施步骤如下：

一：建立目标跟踪系统模型。本发明的目标状态方程和测量方程为：

X_k＝ΦX_k-1+Γv_k-1(k≥1)    (26)

z_k＝HX_k+w_k    (27)

代替z_k，得到：

$z_{m,k}^d=\left[\begin{array}{c}{x}_{m,k}\\ y_{m,k}\\ z_{m,k}\end{array}\right]-{\mathrm{\μ}}_{m,k}=H{X}_k+w_k---\left(28\right)$

其中

$\mathrm{\Γ}=\left[\begin{array}{ccc}{T}^2/2& 0& 0\\ T& 0& 0\\ 0& T^2/2& 0\\ 0& T& 0\\ 0& 0& T^2/2\\ 0& 0& T\end{array}\right];Q_k={\mathrm{\σ}}_v^2\left[\begin{array}{cccccc}{T}^4/4& T^3/2& 0& 0& 0& 0\\ T^3/2& T^2& 0& 0& 0& 0\\ 0& 0& T^4/4& T^3/2& 0& 0\\ 0& 0& T^3/2& T^2& 0& 0\\ 0& 0& 0& 0& T^4/4& T^3/2\\ 0& 0& 0& 0& T^3/2& T^2\end{array}\right];$

$H=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right];\mathrm{\Φ}=\left[\begin{array}{cccccc}1& T& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& T& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

二：对目标状态进行初始化。如图1中的步骤101，设目标初始状态为X₀，初始均值为均方误差为P_0|0，满足：

${\hat{X}}_{0|0}=E\left[X_0\right]---\left(29\right)$

P_0|0＝Var[X₀]    (30)

三：预测状态和相应的协方差。如图1的步骤102，在k-1时刻，通过计算预测得到k时刻的状态预测和其相应的预测协方差：

${\hat{X}}_{k|k-1}=\mathrm{\Φ}{\hat{X}}_{k-1}+{\mathrm{\Γv}}_{k-1}---\left(31\right)$

P_k|k-1＝ΦP_kΦ^T+ΓQ_kΓ^T    (32)

四：更新测量方程。如图1中的步骤103，通过(28)式来更新测量方程：

$z_{m,k}^d=\left[\begin{array}{c}{x}_{m,k}\\ y_{m,k}\\ z_{m,k}\end{array}\right]-{\mathrm{\μ}}_{m,k}=\left[\begin{array}{c}({1-e}^{-{\mathrm{\σ}}_{\mathrm{\θ}}^2-{\mathrm{\σ}}_{\mathrm{\β}}^2}+e^{({-\mathrm{\σ}}_{\mathrm{\θ}}^2/2-{\mathrm{\σ}}_{\mathrm{\β}}^2/2)})r_{m,k}\cos {\mathrm{\β}}_{m,k}\cos {\mathrm{\θ}}_{m,k}\\ (1-e^{-{\mathrm{\σ}}_{\mathrm{\θ}}^2-{\mathrm{\σ}}_{\mathrm{\β}}^2}+e^{({-\mathrm{\σ}}_{\mathrm{\θ}}^2/2-{\mathrm{\σ}}_{\mathrm{\β}}^2/2)})r_{m,k}\cos {\mathrm{\β}}_{m,k}\sin {\mathrm{\θ}}_{m,k}\\ ({1-e}^{{-\mathrm{\σ}}_{\mathrm{\β}}^2}-e^{({-\mathrm{\σ}}_{\mathrm{\β}}^2/2)})r_{m,k}\sin {\mathrm{\β}}_{m,k}\end{array}\right]---\left(33\right)$

五：更新计算测量误差的均方差矩阵。如图1中的步骤104，根据(9---14)式中的r_p,θ_p,β_p可以通过在雷达直角坐标系下的卡尔曼滤波预测的目标位置(x_p,y_p,z_p)转换获得:

$\begin{array}{c}{r}_p=\sqrt{x_p^2+y_p^2+z_p^2}\\ {\mathrm{\θ}}_p=\arctan \frac{y_p}{x_p}\\ {\mathrm{\β}}_p=\arctan \frac{z_p}{\sqrt{x_p^2+y_p^2}}\end{array}---\left(34\right)$

假设雷达直角坐标系下的卡尔曼滤波预测的目标位置满足关系式：

$\begin{array}{c}{x=x}_p+{\tilde{x}}_p\\ {y=y}_p+{\tilde{y}}_p\\ {z=z}_p+{\tilde{z}}_p\end{array}---\left(35\right)$

其中，为相互独立的高斯噪声，卡尔曼滤波是无偏估计，所以可以推导出随机向量的期望值和方差。

已知本发明所选用的目标状态方程和测量方程，则随机向量的期望值和方差为：

$\begin{array}{c}E\left[{({\tilde{x}}_p,{\tilde{y}}_p,{\tilde{z}}_p)}^T\right]=0\\ \mathrm{cov}\left[{({\tilde{x}}_p,{\tilde{y}}_p,{\tilde{z}}_p)}^T\right]={\mathrm{HP}}_{k|k-1}{H}^T\end{array}---\left(36\right)$

因此，式(9)到式(14)中R的具体表达式中，(x_p,y_p,z_p)为已知量，则(r_p,θ_p,β_p)为已知量；未知，则为未知量，那么R可以表示为一个非线性函数：

$R=f({\mathrm{\σ}}_{r_p},E\left({\tilde{r}}_p\right),E\left(\cos 2{\widetilde{\mathrm{\θ}}}_p\right),E\left(\sin 2{\widetilde{\mathrm{\θ}}}_p\right),E\left(\cos {\widetilde{\mathrm{\θ}}}_p\right),E\left(\sin {\widetilde{\mathrm{\θ}}}_p\right),E\left(\cos 2{\widetilde{\mathrm{\β}}}_p\right),E\left(\sin 2{\widetilde{\mathrm{\β}}}_p\right))---\left(37\right)$

可以由(x_p,y_p,z_p)和表示为：

${\tilde{r}}_p=\sqrt{x_p^2+y_p^2+z_p^2}-\sqrt{{(x_p+{\tilde{x}}_p)}^2+{(y_p+{\tilde{y}}_p)}^2+{(z_p+{\tilde{z}}_p)}^2}---\left(38\right)$

$\cos 2{\widetilde{\mathrm{\θ}}}_p=\cos 2(\arctan \frac{y_p}{x_p}-\arctan \frac{y_p+{\tilde{y}}_p}{x_p+{\tilde{x}}_p})---\left(39\right)$

$\sin 2{\widetilde{\mathrm{\θ}}}_p=\sin 2(\arctan \frac{y_p}{x_p}-\arctan \frac{y_p+{\tilde{y}}_p}{x_p+{\tilde{x}}_p})---\left(40\right)$

$\cos {\widetilde{\mathrm{\θ}}}_p=\cos (\arctan \frac{y_p}{x_p}-\arctan \frac{y_p+{\tilde{y}}_p}{x_p+{\tilde{x}}_p})---\left(41\right)$

$\sin {\widetilde{\mathrm{\θ}}}_p=\sin (\arctan \frac{y_p}{x_p}-\arctan \frac{y_p+{\tilde{y}}_p}{x_p+{\tilde{x}}_p})---\left(42\right)$

$\sin 2{\widetilde{\mathrm{\β}}}_p=\sin 2(\arctan \frac{z_p}{\sqrt{x_p^2+y_p^2}}-\arctan \frac{z_p+{\tilde{z}}_p}{\sqrt{{(x_p+{\tilde{x}}_p)}^2+{(y_p+{\tilde{y}}_p)}^2}})---\left(43\right)$

$\cos 2{\widetilde{\mathrm{\β}}}_p=\cos 2(\arctan \frac{z_p}{\sqrt{x_p^2+y_p^2}}-\arctan \frac{z_p+{\tilde{z}}_p}{\sqrt{{(x_p+{\tilde{x}}_p)}^2+{(y_p+{\tilde{y}}_p)}^2}})---\left(44\right)$

式(36)已经求得随机向量的期望值和方差，再通过UT变换计算 的期望值和方差，得到转换测量均方差R的估计值。

采用三阶UT变换的对称采样，得到7个对称采样点的粒子样本集{χ_i,i＝0,1…,6}和粒子相对应的权值，采样原则参考文献，样本集为：

$\left\{{\mathrm{\χ}}_i\right\}=\{0,\sqrt{{3\mathrm{HP}}_{k+1|k}{H}^T},\sqrt{{3\mathrm{HP}}_{k+1|k}{H}^T},-\sqrt{{3\mathrm{HP}}_{k+1|k}{H}^T}\}---\left(45\right)$

对应的权值{w_i}为：

$\left\{w_i\right\}=\{0,\frac{1}{6},\frac{1}{6},\frac{1}{6},\frac{1}{6},\frac{1}{6},\frac{1}{6}\}---\left(46\right)$

(9—14)式通过上述采样点{χ_i,i＝0,1…,6}和对应的权值{w_i}经过UT变换，得到 的期望值和方差，通过式(37)得到R的估计值。

六：计算滤波增益。如图1中的步骤105：

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R)^-1    (47)

七：计算k时刻滤波误差协方差。如图1中的步骤106，可以计算得到k时刻滤波误差协方差：

P_k＝(I-K_kH)P_k|k-1    (48)

八：得到状态滤波估计。如图1中的步骤107，得到状态滤波估计为：

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(z_{m,k}^d-H{\hat{X}}_{k|k-1})---\left(49\right)$

九：判断跟踪是否结束，若是结束退出本方法，否则进入k+1时刻，返回如图1中101继续。

十：参数设定。设目标在匀速运动模型(CV)下，X、Y、Z轴上的过程噪声分别为：q_x＝1、q_y＝1、q_z＝1，仿真帧数为N＝100，蒙特卡罗采样次数M＝500，采样周期为T＝1s。

为更加清晰的描述本发明的跟踪优势，使用如下两组目标位置数据来进行仿真实验比较：

实验一：目标在雷达直角坐标系中初始位置为[10000m,10000m,10000m]，初始速度为[20m/s,20m/s,20m/s]，距离测量精度σ_r＝40m，角度测量精度为σ_θ＝0.01rad、σ_β＝0.01rad时，DCMKF方法和IDCMKF方法的位置均方根误差和速度均方根误差如图2和图3所示。

实验二：目标的初始位置和初始速度分别为：[300km,300km,50km]、[100m/s,100m/s,100m/s]，改变实验一中的角度测量精度，设距离测量精度σ_r＝40m，角度测量精度为σ_θ＝0.1rad、σ_β＝0.1rad时，DCMKF方法和IDCMKF方法的位置均方根误差和速度均方根误差如图4和图5所示。

如图2所示，在开始50帧以内，DCMKF比IDCMKF的跟踪性能好，50帧之后，两种滤波方法的跟踪性能基本一致，并收敛精度为80m；如图3所示，IDCMKF的速度均方根误差收敛速度更快，在20帧趋于稳定，DCMKF于40帧趋于稳定，但两种滤波方法的收敛精度基本一样。实验结果说明在实验一的参数条件下，DCMKF方法和IDCMKF方法的跟踪性能几乎一致。

如图4所示，在增大目标观测距离、方位角和俯仰角时，DCMKF方法在开始几帧内很不稳定，发生跳动，整体跟踪性能不如改进后的滤波方法，DCMKF稳定后收敛值为2200m，IDCMKF稳定收敛值为1500m，说明IDCMKF的跟踪性能优于DCMKF；如图5所示，IDCMKF的速度均方根误差收敛速度更快，在40帧趋于稳定，DCMKF于60帧趋于稳定，但两种滤波方法的收敛精度基本一样。

通过实验得到在在增大目标观测距离、方位角和俯仰角时，本发明的跟踪性能及稳定性优于传统的DCMKF方法。

在此说明书中，本发明已参照特定的实施实例做了描述。但是，很显然仍可以做出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。

Claims (4)

1.一种改进的去偏坐标转换卡尔曼滤波方法，包括以下步骤：

步骤1、建立系统模型，设定本方法的目标的状态方程和测量方程；

步骤2、对目标初始状态和误差协方差进行初始化；

步骤3、在k-1时刻，通过系统模型对k时刻目标的状态和误差协方差进行预测；

步骤4、通过新的观测值对测量方程进行更新；

步骤5、对测量误差的均方误差进行更新；

步骤6、使用卡尔曼标准滤波方法进行滤波估计。

2.根据权利要求1所述的改进的去偏坐标转换卡尔曼滤波方法，其特征在于：步骤4中对测量值进行转换时，本发明采用乘性无偏转换方法对量测值进行转换，使得转换量测值的期望与目标真实值相等，满足如下关系式：

E[x_m|r,θ,β]-x＝0

E[y_m|r,θ,β]-y＝0

E[z_m|r,θ,β]-z＝0

其中，x、y、z分别为目标在直角坐标系中的真实位置，r、β、θ为目标在极坐标下的真实位置，x_m、y_m、z_m为坐标转换得到的直角坐标系中X、Y、Z轴上的测量值。

3.根据权利要求1所述的改进的去偏坐标转换卡尔曼滤波方法，其特征在于：步骤5采用极坐标系下利用卡尔曼滤波预测得到的目标的预测距离为r_p、方位角为θ_p、俯仰角为β_p，来估计转换测量误差的均方差矩阵R，并引入无迹转换方法来计算获得R中的元素值。

4.根据权利要求1所述的改进的去偏坐标转换卡尔曼滤波方法，其特征在于：步骤6使用标准卡尔曼对转换测量值进行滤波，获得目标的状态估计，实现目标跟踪。