CN106383345A - 量测时间间隔不等情况下的滤波器初始化方法 - Google Patents

Abstract

本发明公开了一种量测时间间隔不等情况下的滤波器初始化方法，属于雷达数据处理领域。利用卡尔曼类滤波算法对目标进行跟踪，首先需要对滤波器进行初始化处理，包括滤波状态初始化和滤波协方差阵初始化等。基于三时刻量测差分的滤波器初始化方法存在一个明显的缺陷，即当量测数据的时间间隔不等时无法直接其进行滤波器的初始化，难以适应信噪比低以及航迹起始不确定性等问题造成量测数据的间断或丢失的情况。本发明提出的量测时间间隔不等情况下的滤波器初始化方法立足于解决此类问题，对于有效增强卡尔曼类跟踪滤波算法的适应性具有重要的意义。实施例的实验仿真结果也表明本发明可以有效克服三时刻量测差滤波器初始化方法应用的局限性，因此本发明公开的量测时间间隔不等情况下的滤波器初始化方法具有较强的工程应用价值和推广前景。

Description

量测时间间隔不等情况下的滤波器初始化方法

技术领域

本发明涉及一种雷达数据处理方法，特别是涉及一种雷达量测数据时间间隔不等情况下基于卡尔曼类跟踪滤波(包括卡尔曼滤波、扩展卡尔曼滤波、量测转换卡尔曼滤波以及不敏卡尔曼滤波等)的滤波状态和滤波协方差阵的初始化方法，适应于雷达目标跟踪滤波领域。

背景技术

利用卡尔曼类滤波算法对目标进行跟踪，首先需要对滤波器进行初始化处理，包括滤波状态初始化和滤波协方差阵初始化等。在利用雷达对目标进行探测跟踪时，由于目标信噪比低以及航迹起始的不确定性等问题可能造成量测数据的间断和丢失，从而导致用于滤波器初始化的量测数据时间间隔的不同，进而导致滤波器无法直接利用这些量测数据进行滤波器的初始化以及后续的目标跟踪滤波。因此，实现量测数据时间间隔不等情况下的滤波器直接初始化，对于增强卡尔曼类跟踪滤波算法的适应性具有重要的意义。目前的卡尔曼类跟踪滤波算法一般先对量测数据进行插值或平滑，得到时间间隔相等的量测数据，然后在此基础上采用三时刻量测差分法进行滤波器的初始化。该方法主要通过以下步骤实现：

(1)确定用于滤波器初始化的三个时刻量测；

(2)利用插值或平滑的方法获得时间间隔相等的三个时刻量测数据；

(3)利用差分法得到初始滤波状态，并利用坐标转换将三个时刻雷达的量测误差换为直角坐标系下的量测误差，并计算得到初始滤波协方差阵。

基于三时刻量测差分法的滤波器的初始化方法存在一个比较明显的缺陷：该方法通过对时间间隔不等的量测数据进行插值或平滑，得到时间间隔相等的量测数据，由于量测数据个数较少，插值和平滑过程会引入较大的误差，将导致滤波收敛速度较慢以及跟踪过程中协方差不能客观反映跟踪精度的现象。

发明内容

本发明的目的是提出一种量测时间间隔不等情况下的滤波器初始化方法，解决量测数据时间间隔不等情况下的卡尔曼类滤波器的直接初始化问题。

本发明提出的量测时间间隔不等情况下的滤波器初始化方法的技术方案包括以下步骤：步骤1：利用3/4逻辑法进行航迹起始，确定用于滤波器初始化的三点量测Z(t₁)＝(ρ₁,θ₁,β₁,t₁)，Z(t₂)＝(ρ₂,θ₂,β₂,t₂)，Z(t₃)＝(ρ₃,θ₃,β₃,t₃)，其中ρ_i、θ_i、β_i和t_i分别表示雷达测得目标的距离、方位角、俯仰角以及测量时刻，i＝1,2,3；

步骤2：令

$X\left(t_i\right)=\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ t_i\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ρ}}_{i}cos{\mathrm{\θ}}_{i}cos{\mathrm{\β}}_i\\ {\mathrm{\ρ}}_i{\mathrm{sin\θ}}_i{\mathrm{cos\β}}_i\\ {\mathrm{\ρ}}_i{\mathrm{sin\β}}_i\\ t_i\end{array}\right],i=1,2,3$

将雷达极坐标系下的三点量测Z(t₁)，Z(t₂)，Z(t₃)分别转换成直角坐标系下的量测X(t₁)，X(t₂)，X(t₃)；

步骤3：令

$R\left(t_i\right)=A_i{\mathrm{RA}}_i^T=\left[\begin{array}{ccc}{r}_{11}\left(t_i\right)& r_{12}\left(t_i\right)& r_{13}\left(t_i\right)\\ r_{21}\left(t_i\right)& r_{22}\left(t_i\right)& r_{23}\left(t_i\right)\\ r_{31}\left(t_i\right)& r_{32}\left(t_i\right)& r_{33}\left(t_i\right)\end{array}\right],i=1,2,3$

将雷达极坐标下的量测误差矩阵R转换成直角坐标系下的量测误差矩阵R(t₁)，R(t₂)和R(t₃)，其中

$A_i=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_i{\mathrm{cos\β}}_i& -{\mathrm{\ρ}}_i{\mathrm{sin\θ}}_i{\mathrm{cos\β}}_i& -{\mathrm{\ρ}}_i{\mathrm{cos\θ}}_i{\mathrm{sin\β}}_i\\ {\mathrm{sin\θ}}_i{\mathrm{cos\β}}_i& {\mathrm{\ρ}}_i{\mathrm{cos\θ}}_i{\mathrm{cos\β}}_i& -{\mathrm{\ρ}}_i{\mathrm{sin\θ}}_i{\mathrm{sin\β}}_i\\ {\mathrm{sin\β}}_i& 0& {\mathrm{\ρ}}_i{\mathrm{cos\β}}_i\end{array}\right],i=1,2,3$

和

$R=\left[\begin{array}{ccc}{\mathrm{\σ}}_R^2& 0& 0\\ 0& {\mathrm{\σ}}_{\mathrm{\θ}}^2& 0\\ 0& 0& {\mathrm{\σ}}_{\mathrm{\β}}^2\end{array}\right]$

和分别为雷达在距离、方位、俯仰上的量测误差协方差，为A_i的转置；

步骤4：令T₁＝t₂-t₁，T₂＝t₃-t₂，计算初始滤波状态

$\hat{X}\left(3\right|3)=\left[\begin{array}{c}{x}_3\\ (x_3-x_2)/T_2\\ \mathrm{\[}(x_3-x_2)/T_2-(x_2-x_1)/T_1\]/\[(T_1+T_2)/2\]\\ y_3\\ (y_3-y_2)/T_2\\ \mathrm{\[}(y_3-y_2)/T_2-(y_2-y_1)/T_1\]/\[(T_1+T_2)/2\]\\ z_3\\ (z_3-z_2)/T_2\\ \mathrm{\[}(z_3-z_2)/T_2-(z_2-z_1)/T_1\]/\[(T_1+T_2)/2\]\end{array}\right]$

和初始滤波协方差阵

$\hat{P}\left(3\right|3)=\left[\begin{array}{ccc}{P}_{11}& P_{12}& P_{13}\\ P_{21}& P_{22}& P_{23}\\ P_{31}& P_{32}& P_{33}\end{array}\right]$

其中

$\begin{array}{c}{P}_{ij}=\left[\begin{array}{ccc}{r}_{ij}\left(t_3\right)& \frac{r_{ij}\left(t_3\right)}{T_2}& \frac{2r_{ij}\left(t_3\right)}{(T_1+T_2)T_2}\\ \frac{r_{ij}\left(t_3\right)}{T_2}& \frac{r_{ij}\left(t_3\right)+r_{ij}\left(t_2\right)}{{T_2}^2}& \frac{r_{ij}\left(t_3\right)/T_2+r_{ij}\left(t_2\right)\frac{T_1+T_2}{T_1{T}_2}}{\frac{1}{2}(T_1+T_2)T_2}\\ \frac{2r_{ij}\left(t_3\right)}{(T_1+T_2)T_2}& \frac{r_{ij}\left(t_3\right)/T_2+r_{ij}\left(t_2\right)\frac{T_1+T_2}{T_1{T}_2}}{\frac{1}{2}(T_1+T_2)T_2}& \frac{r_{ij}\left(t_3\right)/{T_2}^2+r_{ij}\left(t_2\right){\left(\frac{T_1+T_2}{T_1{T}_2}\right)}^2+r_{ij}\left(t_1\right)/{T_1}^2}{{\left(\frac{T_1+T_2}{2}\right)}^2}\end{array}\right]\\ i=1,2,3;j=1,2,3\end{array},$

步骤5：得到初始状态和初始协方差阵进行滤波器的初始化。

和背景技术相比，本发明的有益效果说明：

本发明提出的雷达采样间隔不等情况下的滤波器初始化方法，可以直接利用量测时间间隔不等的数据进行滤波器的初始化，避免了传统方法需要首先对量测数据进行插值或平滑获得时间间隔相等的量测数据的过程，从而避免了插值或平滑过程引入的较大误差，扩大了卡尔曼类滤波器的适应范围。

附图说明

附图1是本发明提出的量测时间间隔不等情况下的滤波器初始化方法的整体流程图；

附图2是本发明实施例中目标位置量测误差、位置跟踪误差、位置误差协方差比较图；

附图3是本发明实施例中目标速度跟踪误差、速度跟踪误差协方差比较图；

附图4是本发明实施例中目标加速度跟踪误差、加速度跟踪误差协方差比较图。

具体实施方式

下面结合附图对本发明提出的量测时间间隔不等情况下的滤波器初始化方法进行详细描述。

假设单目标在(x,y,z)平面匀加速直线运动，初始位置(0m，5000m，0m)，x方向速度100m/s，y方向速度为100m/s，z方向速度为0m/s，x方向加速度5s/m²，y方向加速度为2s/m²，z方向速度为0s/m²。雷达不动，位置坐标为(0m，0m，0m)，量测误差在距离、方位、俯仰上的均方根分别为50m、0.2°、0.2°。3/4逻辑法航迹起始得到的三点量测如表1所示。

本发明具体步骤如附图1所示。

(1)按发明内容部分步骤1所述的方法进行3/4逻辑法航迹起始，确定用于滤波器初始化的三点量测，如表1所示；

表1航迹起始确认的三点量测

	1	2	3
				距离(m)	5184.2913	5155.5924	5719.8217
方位角(rad)	1.5535	1.5294	1.4446
				俯仰角(rad)	-0.0009	-0.0010	-0.0028
时标(s)	1	2	6

(2)按发明内容部分步骤2所述的方法将雷达极坐标系下的量测数据转换成直角坐标系下的量测数据，如表2所示；

表2坐标转换后的三点量测

	1	2	3
				x(m)	89.5951	213.5742	719.9668
y(m)	5183.5149	5151.1641	5674.3062
				z(m)	-4.6823	-5.1874	-16.0606
时标(s)	1	2	6

(3)按发明内容部分步骤3所述的方法将雷达极坐标下的量测误差矩阵转换成直角坐标系下的量测误差矩阵

$R\left(t_1\right)=\left[\begin{array}{ccc}328.1352& 37.5399& -0.0339\\ 37.5399& 2499.3494& -1.9618\\ -0.0339& -1.9618& 327.4884\end{array}\right]$

$R\left(t_2\right)=\left[\begin{array}{ccc}327.6040& 90.0703& -0.0907\\ 90.0703& 2496.2634& -2.1877\\ -0.9070& -2.1877& 323.8731\end{array}\right]$

和

$R\left(t_3\right)=\left[\begin{array}{ccc}378.3453& 177.5267& 0.6027\\ 177.5267& 2485.1211& 7.2032\\ 0.6027& 7.2032& 363.5195\end{array}\right]$

(4)按发明内容部分步骤4所述的方法计算初始滤波状态和初始滤波协方差阵

$\hat{X}\left(3\right|3)=\left[\begin{array}{c}719.9668\\ 126.5981\\ 1.0476\\ 5674.3032\\ 130.7855\\ 65.2545\\ -16.0606\\ -2.7183\\ -0.8853\end{array}\right]$

和

$\hat{P}\left(3\right|3)=\left[\begin{array}{ccccccccc}328.1352& \mathrm{164}\mathrm{.0676}& \mathrm{109}\mathrm{.3784}& 37.5399& 18.7699& 12.5133& -0.0339& -0.0170& -0.0113\\ 164.0676& \mathrm{163}\mathrm{.9350}& \mathrm{218}\mathrm{.4917}& 18.7699& 31.9025& 51.2918& -0.0170& -0.0312& -0.0510\\ \mathrm{109}.\mathrm{3784}& \mathrm{218}\mathrm{.4917}& 0\mathrm{.5322}& 12.5133& 51.2918& 173.1422& -0.0113& -0.0510& 0.1734\\ \mathrm{37}\mathrm{.5399}& \mathrm{18}\mathrm{.7699}& \mathrm{12}.5133& 2499.3494& 1249.6747& 833.1165& -1.9618& -0.9809& -0.6539\\ \mathrm{18}\mathrm{.7699}& \mathrm{31}\mathrm{.9025}& 51.2918& 1249.6747& 1248.9032& 1664.6899& -0.9809& -1.0374& -1.4208\\ \mathrm{12}\mathrm{.5133}& \mathrm{51}\mathrm{.2918}& 0.1731& 833.1165& 1664.6899& 3878.4671& -0.6539& -1.4208& 0.7958\\ -0.339& -0.\mathrm{0170}& -0.0113& -1.9618& -0.9809& -0.6539& 327.4884& 163.7442& 109.1628\\ -0.\mathrm{0170}& -0.\mathrm{0312}& -0.0510& -0.9809& -1.0374& -1.4208& 163.7442& 162.8404& 216.5179\\ -0.\mathrm{0113}& -0.\mathrm{0510}& 0.1734& -0.6539& -1.4208& 0.7958& 109.1628& 216.5179& 521.8249\end{array}\right]$

(5)按发明内容部分步骤5所述的方法得到初始状态和初始协方差阵，进行滤波器的初始化。

实施例条件中，雷达量测数据的时间间隔不相等，本发明方法能在雷达三点量测时间间隔不等的情况下进行正常的滤波初始化，且在跟踪过程中目标的位置误差、速度误差和加速度误差都能在在误差协方差中实时正确反应出来(见图2，图3，图4)，从而解决了雷达量测时间间隔不相等情况下卡尔曼类滤波器的初始化问题。

Claims (1)

1.量测时间间隔不等情况下的滤波器初始化方法，其特征包括以下步骤：

步骤1：利用3/4逻辑法进行航迹起始，确定用于滤波器初始化的三点量测Z(t₁)＝(ρ₁,θ₁,β₁,t₁)，Z(t₂)＝(ρ₂,θ₂,β₂,t₂)，Z(t₃)＝(ρ₃,θ₃,β₃,t₃)，其中ρ_i、θ_i、β_i和t_i分别表示雷达测得目标的距离、方位角、俯仰角以及测量时刻，i＝1,2,3；

步骤2：令

$X\left(t_i\right)=\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ t_i\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ρ}}_i{\mathrm{cos\θ}}_i{\mathrm{cos\β}}_i\\ {\mathrm{\ρ}}_i{\mathrm{sin\θ}}_i{\mathrm{cos\β}}_i\\ {\mathrm{\ρ}}_i{\mathrm{sin\β}}_i\\ t_i\end{array}\right],i=1,2,3$

将雷达极坐标系下的三点量测Z(t₁)，Z(t₂)，Z(t₃)分别转换成直角坐标系下的量测X(t₁)，X(t₂)，X(t₃)；

步骤3：令

$R\left(t_i\right)=A_i{\mathrm{RA}}_i^T=\left[\begin{array}{ccc}{r}_{11}\left(t_i\right)& r_{12}\left(t_i\right)& r_{13}\left(t_i\right)\\ r_{21}\left(t_i\right)& r_{22}\left(t_i\right)& r_{23}\left(t_i\right)\\ r_{31}\left(t_i\right)& r_{32}\left(t_i\right)& r_{33}\left(t_i\right)\end{array}\right],i=1,2,3$

将雷达极坐标下的量测误差矩阵R转换成直角坐标系下的量测误差矩阵R(t₁)，R(t₂)和R(t₃)，其中

$A_i=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_i{\mathrm{cos\β}}_i& -{\mathrm{\ρ}}_i{\mathrm{sin\θ}}_i{\mathrm{cos\β}}_i& -{\mathrm{\ρ}}_i{\mathrm{cos\θ}}_i{\mathrm{sin\β}}_i\\ {\mathrm{sin\θ}}_i{\mathrm{cos\β}}_i& {\mathrm{\ρ}}_i{\mathrm{cos\θ}}_i{\mathrm{cos\β}}_i& -{\mathrm{\ρ}}_i{\mathrm{sin\θ}}_i{\mathrm{sin\β}}_i\\ {\mathrm{sin\β}}_i& 0& {\mathrm{\ρ}}_i{\mathrm{cos\β}}_i\end{array}\right],i=1,2,3$

和

$R=\left[\begin{array}{ccc}{\mathrm{\σ}}_R^2& 0& 0\\ 0& {\mathrm{\σ}}_{\mathrm{\θ}}^2& 0\\ 0& 0& {\mathrm{\σ}}_{\mathrm{\β}}^2\end{array}\right]$

和分别为雷达在距离、方位、俯仰上的量测误差协方差，为A_i的转置；

步骤4：令T₁＝t₂-t₁，T₂＝t₃-t₂，计算初始滤波状态

$\hat{X}\left(3|3\right)=\left[\begin{array}{c}{x}_3\\ \left(x_3-x_2\right)/T_2\\ \[\left(x_3-x_2\right)/T_2-\left(x_2-x_1\right)/T_1\]/\[\left(T_1+T_2\right)/2\]\\ y_3\\ \left(y_3-y_2\right)/T_2\\ \[\left(y_3-y_2\right)/T_2-\left(y_2-y_1\right)/T_1\]/\[\left(T_1+T_2\right)/2\]\\ z_3\\ \left(z_3-z_2\right)/T_2\\ \[\left(z_3-z_2\right)/T_2-\left(z_2-z_1\right)/T_1\]/\[\left(T_1+T_2\right)/2\]\end{array}\right]$

和初始滤波协方差阵

$\hat{P}\left(3|3\right)=\left[\begin{array}{ccc}{P}_{11}& P_{12}& P_{13}\\ P_{21}& P_{22}& P_{23}\\ P_{31}& P_{32}& P_{33}\end{array}\right]$

其中

$\begin{array}{c}{P}_{ij}=\left[\begin{array}{ccc}{r}_{ij}\left(t_3\right)& \frac{r_{ij}\left(t_3\right)}{T_2}& \frac{2r_{ij}\left(t_3\right)}{\left(T_1+T_2\right)T_2}\\ \frac{r_{ij}\left(t_3\right)}{T^2}& \frac{r_{ij}\left(t_3\right)+r_{ij}\left(t_2\right)}{{T_2}^2}& \frac{r_{ij}\left(t_3\right)/T_2+r_{ij}\left(t_2\right)\frac{T_1+T_2}{T_1{T}_2}}{\frac{1}{2}\left(T_1+T_2\right)T_2}\\ \frac{2r_{ij}\left(t_3\right)}{\left(T_1+T_2\right)T_2}& \frac{r_{ij}\left(t_3\right)/T_2+r_{ij}\left(t_2\right)\frac{T_1+T_2}{T_1{T}_2}}{\frac{1}{2}\left(T_1+T_2\right)T_2}& \frac{r_{ij}\left(t_3\right)/{T_2}^2+r_{ij}\left(t_2\right){\left(\frac{T_1+T_2}{T_1{T}_2}\right)}^2+r_{ij}\left(t_1\right)/{T_1}^2}{{\left(\frac{T_1+T_2}{2}\right)}^2}\end{array}\right]\\ i=1,2,3;j=1,2,3\end{array},$

步骤5：得到初始状态和初始协方差阵进行滤波器的初始化。