[发明专利]ECEF坐标系下2-D雷达误差配准两步算法

权利要求书

1.一种基于ECEF坐标系的两坐标雷达误差配准方法，其特征在于包括以下步骤：

步骤1：雷达i(i取A、B，下同)将k时刻本地地理坐标：纬度Ls_i，经度Rs_i，高度H_Si和对目标的量测值：距离r_i，方位角θ_i上报到融合中心；

步骤2：融合中心使用雷达A、B上报的信息进行处理，得到等效量测方程；

在融合中心数据处理计算机中执行以下步骤：

(1)分别计算雷达i在ECEF坐标系中的坐标和旋转矩阵：

xis(k)=(Ci(k)+Hsi(k))cos(Lsi(k))cos(λsi(k))yis(k)=(Ci(k)+Hsi(k))cos(Lsi(k))sin(λsi(k))zis(k)=(Ci(k)(1-e2)+Hsi(k))sin(Lsi(k))---(1)]]>

其中，Eq＝6378137.0，b＝6356752.3142；

Ti(k)=-sin(Rsi(k))-sin(Lsi(k))cos(Rsi(k))cos(Lsi(k))cos(Rsi(k))cos(Rsi(k))-sin(Lsi(k))sin(Rsi(k))cos(Lsi(k))sin(Rsi(k))0cos(Lsi(k))sin(Lsi(k))---(2)]]>

Di1(k)=Ti(k)-sin(θi(k))-ri(k)cos(θi(k))-cos(θi(k))ri(k)sin(θi(k))00---(3)]]>

Z1(k)=xBS(k)-xAS(k)yBS(k)-yAS(k)0+TB(k)rB(k)sin(θB(k))rB(k)cos(θB(k))0-TA(k)rA(k)sin(θA(k))rA(k)cos(θA(k))0---(4)]]>

F₁(k)＝G₁(k)＝[D_A1(k)-D_B1(k)]                       (5)

步骤3：Kalman滤波初始化

(1)Kalman滤波值初始化

β^(k|k)=(0,0,0,0)T---(6)]]>

为系统偏差估计值，为1×4向量，分别表示Δr_A(k)、Δθ_A(k)、Δr_B(k)、Δθ_B(k)，这里k＝1，即在第1时刻进行初始化，上标“^T”表示矩阵转置；

(2)Kalman滤波协方差矩阵P(k|k)初始化

P(k|k)=10700001000010700001---(7)]]>

步骤4：Kalman滤波更新

(1)状态预测值为：

β^(k+1|k)=β^(k|k)---(8)]]>

(2)量测预测值为：

Z^1(k+1|k)=F1(k+1)β^(k+1|k)---(9)]]>

(3)新息协方差矩阵为：

S(k+1)=F1(k+1)P(k|k)F1T(k+1)+G1(k+1)RG1T(k+1)---(10)]]>

其中，R表示雷达量测噪声方差阵，为4×4对角阵，分别为雷达A距离量测噪声方差，雷达A方位角量测噪声方差，雷达B距离量测噪声方差，雷达B方位角量测噪声方差；

(4)增益矩阵为：

K(k+1)=P(k|k)F1T(k+1)S-1(k+1)---(11)]]>

(5)状态估计更新

β^(k+1|k+1)=β^(k+1|k)+K(k+1)(Z1(k+1)-Z^1(k+1|k))---(12)]]>

(6)状态估计协方差更新

P(k+1|k+1)=(I(4)-K(k+1)F1(k+1))P(k|k)(I(4)-K(k+1)F1(k+1))T]]>

+K(k+1)G1(k+1)RG1T(k+1)KT(k+1)---(13)]]>

其中，I(4)为4×4单位阵；

步骤5：对计算结果进行修正

(1)设置高度值；

H＝90000m

RnA(k+1)=-12H2rA(k+1)-18H4rA3(k+1)---(14)]]>

RnB(k+1)=-12H2rB(k+1)-18H4rB3(k+1)]]>

(2)生成等效方程

Di2(k+1)=Ti0-Rni(k+1)cos(θi(k+1))0Rni(k+1)sin(θi(k+1))00---(15)]]>

Z2(k+1)=00zBs-zAs+TB(k+1)RnB(k+1)sin(θB(k+1))RnB(k+1)cos(θB(k+1))H-TA(k+1)RnA(k+1)sin(θA(k+1))RnA(k+1)cos(θA(k+1))H---(16)]]>

F₂(k+1)＝[D_A2(k+1)-D_B2(k+1)]                  (17)

(3)计算修正量

Z(k+1)=Z2(k+1)-F2(k+1)×β^(k+1)---(18)]]>

a1=F1(1,1)+F2(1,1)F1(1,3)+F2(1,3)F1(2,1)+F2(2,1)F1(2,3)+F2(2,3)---(19)]]>

b1=Z(1,1)F1(1,3)+F2(1,3)Z(2,1)F1(2,3)+F2(2,3)---(20)]]>

c1=F1(1,1)+F2(1,1)Z(1,1)F1(2,1)+F2(2,1)Z(2,1)---(21)]]>

其中，F_i(u，v)表示矩阵F_i(k+1)的第u行第v列元素；

Δβ^(1)=det(b1)/det(a1)---(22)]]>

Δβ^(3)=det(c1)/det(a1)---(23)]]>

其中，det(A)表示求矩阵A的行列式；

(4)对解的可信度进行判断

如果c，则否则

如果则否则

其中，表示第i行元素；

步骤6：令k＝k+1，重复执行步骤1、2、4、5进行下一周期的系统偏差估计，直至雷达关机，其中，步骤4中的状态估计初始值和状态估计协方差为上一次Kalman滤波得到的状态估计值和状态估计协方差值，而非步骤5得到的修正后的状态估计值和状态估计协方差值。