ECEF坐标系下2-D雷达误差配准两步算法

摘要

本发明公开了一种基于ECEF坐标系的两坐标雷达系统偏差估计算法，属于雷达数据处理领域。两坐标雷达由于缺少目标的高度信息所以不能直接使用ECEF坐标转换公式，因此只能在平面内进行误差配准，此时，当目标、雷达距离较远时，由于地球曲率的影响，这种平面内的配准方法势必造成很大的坐标转换误差而影响误差配准的精度，本发明立足于解决此类现实问题。首先，假设两个雷达对目标的量测高度为零，使用ECEF坐标系转换公式进行误差配准，得到初步的估计值，然后使用修正算法对此估计结果进行修正。利用该发明可以有效提高雷达系统偏差的估计精度，工程实现容易，具有推广应用价值，可应用于雷达、ESM、红外、IFF等传感器的误差配准，还可应用于医学、交通等相关领域的数据处理。

主权项

1.一种基于ECEF坐标系的两坐标雷达误差配准方法，其特征在于包括以下步骤：步骤1：雷达i(i取A、B，下同)将k时刻本地地理坐标：纬度Ls_i，经度Rs_i，高度H_Si和对目标的量测值：距离r_i，方位角θ_i上报到融合中心；步骤2：融合中心使用雷达A、B上报的信息进行处理，得到等效量测方程；在融合中心数据处理计算机中执行以下步骤：(1)分别计算雷达i在ECEF坐标系中的坐标和旋转矩阵：$\left\{\begin{array}{c}{x}_{\mathrm{is}}\left(k\right)=(C_i\left(k\right)+H_{\mathrm{si}}\left(k\right))\cos \left(L_{\mathrm{si}}\left(k\right)\right)\cos \left({\lambda }_{\mathrm{si}}\left(k\right)\right)\\ y_{\mathrm{is}}\left(k\right)=(C_i\left(k\right)+H_{\mathrm{si}}\left(k\right))\cos \left(L_{\mathrm{si}}\left(k\right)\right)\sin \left({\lambda }_{\mathrm{si}}\left(k\right)\right)\\ z_{\mathrm{is}}\left(k\right)=(C_i\left(k\right)(1-e^2)+H_{\mathrm{si}}\left(k\right))\sin \left(L_{\mathrm{si}}\left(k\right)\right)\end{array}\right.---\left(1\right)$其中，Eq＝6378137.0，b＝6356752.3142；$T_i\left(k\right)=\left[\begin{array}{ccc}-\sin \left(R_{\mathrm{si}}\left(k\right)\right)& -\sin \left(L_{\mathrm{si}}\left(k\right)\right)\cos \left(R_{\mathrm{si}}\left(k\right)\right)& \cos \left(L_{\mathrm{si}}\left(k\right)\right)\cos \left(R_{\mathrm{si}}\left(k\right)\right)\\ \cos \left(R_{\mathrm{si}}\left(k\right)\right)& -\sin \left(L_{\mathrm{si}}\left(k\right)\right)\sin \left(R_{\mathrm{si}}\left(k\right)\right)& \cos \left(L_{\mathrm{si}}\left(k\right)\right)\sin \left(R_{\mathrm{si}}\left(k\right)\right)\\ 0& \cos \left(L_{\mathrm{si}}\left(k\right)\right)& \sin \left(L_{\mathrm{si}}\left(k\right)\right)\end{array}\right]---\left(2\right)$$D_{i1}\left(k\right)=T_i\left(k\right)\left[\begin{array}{cc}-\sin \left({\theta }_i\left(k\right)\right)& -r_i\left(k\right)\cos \left({\theta }_i\left(k\right)\right)\\ -\cos \left({\theta }_i\left(k\right)\right)& r_i\left(k\right)\sin \left({\theta }_i\left(k\right)\right)\\ 0& 0\end{array}\right]---\left(3\right)$$Z_1\left(k\right)=\left[\begin{array}{c}{x}_{\mathrm{BS}}\left(k\right)-x_{\mathrm{AS}}\left(k\right)\\ y_{\mathrm{BS}}\left(k\right)-y_{\mathrm{AS}}\left(k\right)\\ 0\end{array}\right]{+T}_B\left(k\right)\left[\begin{array}{c}{r}_B\left(k\right)\sin \left({\theta }_B\left(k\right)\right)\\ r_B\left(k\right)\cos \left({\theta }_B\left(k\right)\right)\\ 0\end{array}\right]-T_A\left(k\right)\left[\begin{array}{c}{r}_A\left(k\right)\sin \left({\theta }_A\left(k\right)\right)\\ r_A\left(k\right)\cos \left({\theta }_A\left(k\right)\right)\\ 0\end{array}\right]---\left(4\right)$F₁(k)＝G₁(k)＝[D_A1(k)-D_B1(k)]                       (5)步骤3：Kalman滤波初始化(1)Kalman滤波值初始化$\hat{\beta }\left(k\right|k)={\left(\mathrm{0,0,0,0}\right)}^T---\left(6\right)$为系统偏差估计值，为1×4向量，分别表示Δr_A(k)、Δθ_A(k)、Δr_B(k)、Δθ_B(k)，这里k＝1，即在第1时刻进行初始化，上标“^T”表示矩阵转置；(2)Kalman滤波协方差矩阵P(k|k)初始化$P\left(k\right|k)=\left[\begin{array}{cccc}{10}^7& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& {10}^7& 0\\ 0& 0& 0& 1\end{array}\right]---\left(7\right)$步骤4：Kalman滤波更新(1)状态预测值为：$\hat{\beta }(k+1|k)=\hat{\beta }\left(k\right|k)---\left(8\right)$(2)量测预测值为：${\hat{Z}}_1(k+1|k)=F_1(k+1)\hat{\beta }(k+1|k)---\left(9\right)$(3)新息协方差矩阵为：$S(k+1)=F_1(k+1)P\left(k\right|k)F_1^T(k+1)+G_1(k+1){\mathrm{RG}}_1^T(k+1)---\left(10\right)$其中，R表示雷达量测噪声方差阵，为4×4对角阵，分别为雷达A距离量测噪声方差，雷达A方位角量测噪声方差，雷达B距离量测噪声方差，雷达B方位角量测噪声方差；(4)增益矩阵为：$K(k+1)=P\left(k\right|k)F_1^T(k+1)S^{-1}(k+1)---\left(11\right)$(5)状态估计更新$\hat{\beta }(k+1|k+1)=\hat{\beta }(k+1|k)+K(k+1)(Z_1(k+1)-{\hat{Z}}_1(k+1|k))---\left(12\right)$(6)状态估计协方差更新$P(k+1|k+1)={(I\left(4\right)-K(k+1)F_1(k+1))P\left(k\right|k)(I\left(4\right)-K(k+1)F_1(k+1))}^T$$+K(k+1)G_1(k+1){\mathrm{RG}}_1^T(k+1)K^T(k+1)---\left(13\right)$其中，I(4)为4×4单位阵；步骤5：对计算结果进行修正(1)设置高度值；H＝90000m$R_{\mathrm{nA}}(k+1)=-\frac{1}{2}\frac{H^2}{r_A(k+1)}-\frac{1}{8}\frac{H^4}{r_A^3(k+1)}---\left(14\right)$$R_{\mathrm{nB}}(k+1)=-\frac{1}{2}\frac{H^2}{r_B(k+1)}-\frac{1}{8}\frac{H^4}{r_B^3(k+1)}$(2)生成等效方程$D_{i2}(k+1)=T_i\left[\begin{array}{cc}0& -R_{\mathrm{ni}}(k+1)\cos \left({\theta }_i(k+1)\right)\\ 0& R_{\mathrm{ni}}(k+1)\sin \left({\theta }_i(k+1)\right)\\ 0& 0\end{array}\right]---\left(15\right)$$Z_2(k+1)=\begin{array}{c}\left[\begin{array}{c}0\\ 0\\ z_{\mathrm{Bs}}-z_{\mathrm{As}}\end{array}\right]+T_B(k+1)\left[\begin{array}{c}{R}_{\mathrm{nB}}(k+1)\sin \left({\theta }_B(k+1)\right)\\ R_{\mathrm{nB}}(k+1)\cos \left({\theta }_B(k+1)\right)\\ H\end{array}\right]\end{array}-T_A(k+1)\left[\begin{array}{c}{R}_{\mathrm{nA}}(k+1)\sin \left({\theta }_A(k+1)\right)\\ R_{\mathrm{nA}}(k+1)\cos \left({\theta }_A(k+1)\right)\\ H\end{array}\right]---\left(16\right)$F₂(k+1)＝[D_A2(k+1)-D_B2(k+1)]                  (17)(3)计算修正量$Z(k+1)=Z_2(k+1)-F_2(k+1)\times \hat{\beta }(k+1)---\left(18\right)$$a1=\left[\begin{array}{cc}{F}_1\left(\mathrm{1,1}\right)+F_2\left(\mathrm{1,1}\right)& F_1\left(\mathrm{1,3}\right)+F_2\left(\mathrm{1,3}\right)\\ F_1\left(\mathrm{2,1}\right)+F_2\left(\mathrm{2,1}\right)& F_1\left(\mathrm{2,3}\right)+F_2\left(\mathrm{2,3}\right)\end{array}\right]---\left(19\right)$$b1=\left[\begin{array}{cc}Z\left(\mathrm{1,1}\right)& F_1\left(\mathrm{1,3}\right)+F_2\left(\mathrm{1,3}\right)\\ Z\left(\mathrm{2,1}\right)& F_1\left(\mathrm{2,3}\right)+F_2\left(\mathrm{2,3}\right)\end{array}\right]---\left(20\right)$$c1=\left[\begin{array}{cc}{F}_1\left(\mathrm{1,1}\right)+F_2\left(\mathrm{1,1}\right)& Z\left(\mathrm{1,1}\right)\\ F_1\left(\mathrm{2,1}\right)+F_2\left(\mathrm{2,1}\right)& Z\left(\mathrm{2,1}\right)\end{array}\right]---\left(21\right)$其中，F_i(u，v)表示矩阵F_i(k+1)的第u行第v列元素；$\Delta \hat{\beta }\left(1\right)=\det \left(b1\right)/\det \left(a1\right)---\left(22\right)$$\Delta \hat{\beta }\left(3\right)=\det \left(c1\right)/\det \left(a1\right)---\left(23\right)$其中，det(A)表示求矩阵A的行列式；(4)对解的可信度进行判断如果c，则否则如果则否则其中，表示第i行元素；步骤6：令k＝k+1，重复执行步骤1、2、4、5进行下一周期的系统偏差估计，直至雷达关机，其中，步骤4中的状态估计初始值和状态估计协方差为上一次Kalman滤波得到的状态估计值和状态估计协方差值，而非步骤5得到的修正后的状态估计值和状态估计协方差值。