CN106969767B - 一种动平台传感器系统偏差的估计方法 - Google Patents

Abstract

一种动平台传感器系统偏差的估计方法，包括以下步骤：第一步、基于合作目标建立估计姿态系统偏差的状态空间；第二步、基于非合作目标建立估计姿态系统偏差的状态空间；第三步、实现基于合作目标和非合作目标的交替配准。本发明解决了动平台传感器系统中，传感器量测无偏差而平台姿态角存在偏差的空间配准问题，考虑到地球曲率的影响，基于公共坐标系为地心地固坐标系的三维场景，分析传感器不存在量测偏差的前提下，平台姿态角偏差的估计问题，根据目标类型的不同，设计了基于合作目标为单个或多个的姿态角配准，基于非合作目标为单个或多个的姿态角配准，以及两类目标共存时的姿态角配准，能够精确估计姿态角偏差，实现目标的精确跟踪。

Description

一种动平台传感器系统偏差的估计方法

技术领域

本发明涉及多平台多传感器数据融合领域，具体涉及一种动平台传感器系统偏差的估计方法，从而协调好基于合作目标的信息和基于非合作目标的信息。

背景技术

目前已公开的空间配准文献所研究的问题是针对探测传感器(如雷达)的系统偏差估计，很少提到平台姿态角的系统偏差估计。在超视距作战过程中，由于目标距离很远，很小的姿态偏差都可能导致极大的多源数据探测误差。因此亟需研究平台的姿态配准算法。

在多平台多传感器目标跟踪系统中，由于平台自身的姿态角系统偏差在坐标转换过程中会严重的污染量测数据，使得后续的关联、滤波、融合等运算异常，甚至产生虚假目标，很难发挥出多传感器的优势[1]。目前，对系统偏差估计的算法主要包括：实时质量控制法[2]、最小二乘类估计算法[3]、精确极大似然类法[4]，以及基于kalman滤波的方法[5]、不敏滤波[6]、EM算法[7]等，这些算法均适用于平台姿态角偏差是固定值时的系统偏差估计。

而在空间配准技术中，根据目标类型的不同，主要分为基于合作目标的空间配准和基于非合作目标的空间配准两类[8]。合作目标是指目标的真实位置是已知的，所以只需要一个传感器就能获取其位置。而非合作是指目标的真实位置是未知的，所以往往需要两个或两个以上的传感器对其进行探测。文献[9]研究了当同时出现合作目标与非合作目标时怎样协调好基于合作目标的空间配准结果与非合作目标信息，但其只适用于仅有一个传感器对合作目标有量测的场景，并且是针对仅传感器有系统偏差的情况，而非平台姿态角有偏差。

因此，需要设计一种当合作目标与非合作目标同时存在，仅平台的姿态角存在系统偏差场景下的空间配准方法，从而使得配准精度既优于单基于合作目标的空间配准，同时也优于单基于非合作目标的空间配准，进而实现对目标更加精确的跟踪。

以上内容所提及的参考文献如下：

[1]保铮.传感器信号的长时间积累[J].第七届全国传感器学术年会,南京,1999:9-15；

[2]Burke J.The SAGE real quality control fraction and its interfacewith BUIC II/BUIC III[R].[s.l.]：MITRE Corporation，1966；

[3]Sudano J J.A least square algorithm with covariance weighting forcomputing the translational and rotational errors between two radar sites[J].IEEE AES，1993，29(1)：383-387；

[4]Zhou Yifeng，Henry L.An exact maximum likelihood registrationalgorithm for data fusion[J].IEEE Trans Signal Processing，1997，45(6)：1560-1572；

[5]Kousuge Y，Okada T.Bias Estimation of Two 3-Dimensonal Radars UsingKalman Filter[J].4th Int.Workshop on Advanced Motion Control，1996，1(1)：377-382；

[6]W.Li,H.Leung and Y.Zhou.Space-time registration of radar and ESMusing unscented Kalman filter.IEEE Trans.On AES.Vol.40(3),2004,824-836；

[7]Zhenhua Li and Henry Leung.An Expectation Maximization BasedSimultaneous Registration and Fusion Algorithm for Radar Networks.IEEE CCECE/CCGEI,Ottawa,May 2006；

[8]宋文彬.传感器数据空间配准算法研究进展[J].传感器与微系统,2012,31(8):5-8；

[9]宋文彬.基于合作目标与非合作目标的一体化空间配准新算法[J].电讯技术,2013,53(11):1422-1427。

发明内容

本发明的目的在于针对上述现有技术中的问题，提供一种动平台传感器系统偏差的估计方法，协调好基于合作目标的信息和基于非合作目标的信息，实现对目标的精确跟踪。

为了实现上述目的，本发明采用的技术方案包括以下步骤：

第一步、基于合作目标建立估计姿态系统偏差的状态空间；

将传感器载于平台上，使二者的笛卡尔坐标系重合，取ECEF坐标系为配准的坐标系，得到：X_true＝X_s+R_tR_l(α-Δα,β-Δβ,γ-Δγ)X₁；式中，X_true为目标在ECEF坐标系下的真实位置，X_s为平台在ECEF坐标系下的真实坐标，R_t为东北天坐标系到ECEF坐标系的转换矩阵，R_l为机载平台笛卡尔坐标系到东北天坐标系的转换矩阵，X₁为机载平台笛卡尔坐标系下平台对目标的量测，β,γ,α分别是平台俯仰角、偏航角、滚动角的量测值，Δβ,Δγ,Δα分别是平台俯仰角、偏航角、滚动角的系统偏差；

第二步、基于非合作目标建立估计姿态系统偏差的状态空间；

将传感器载于平台上，使二者的笛卡尔坐标系重合，取ECEF坐标系为配准的坐标系，对运动的非合作目标，在不考虑传感器的系统偏差及平台的姿态角量测噪声的情况下，两平台A,B在同一时刻，对目标的不含偏差量测相同，得到：

X_sA+R_tAR_lA(α_A-Δα_A,β_A-Δβ_A,γ_A-Δγ_A)X_pA

＝X_sB+R_tBR_lB(α_B-Δα_B,β_B-Δβ_B,γ_B-Δγ_B)X_pB

式中，X_sA,X_sB分别为平台A,B在ECEF坐标系下的坐标，R_tA,R_tB分别为平台A,B东北天坐标系到ECEF坐标系的转换矩阵，R_lA,R_lB分别为平台A,B机载平台笛卡尔坐标系到东北天坐标系的转换矩阵，X_pA,X_pB分别为平台A,B在机载平台笛卡尔坐标系下传感器对目标的量测，(α_A,β_A,γ_A)和(α_B,β_B,γ_B)分别是平台A,B的滚动角、俯仰角、偏航角的量测值，(Δα_A,Δβ_A,Δγ_A)和(Δα_B,Δβ_B,Δγ_B)分别是平台A,B相应姿态角的系统偏差；

第三步、实现基于合作目标和非合作目标的交替配准。

所述的第一步中：

1.1)得到东北天坐标系到ECEF坐标系的转换矩阵R_t为：

得到：

其中：

此处(x_s,y_s,z_s)表示机载平台中心在ECEF坐标系中的位置，而(λ_s,l_s,h_s)则表示机载平台中心在大地坐标系的位置，分别表示经度，纬度和高度；

1.2)得到机载平台笛卡尔坐标系到东北天坐标系的转换矩阵R_l(α,β,γ)为：

其中，γ,α,β分别为平台的偏航角，滚动角和俯仰角；

1.3)得到机载平台笛卡尔坐标系到东北天坐标系的真实转换矩阵R_tl为：

其中，Δγ,Δα,Δβ分别为平台的偏航角系统偏差，滚动角系统偏差和俯仰角系统偏差。

将步骤1.3)得到的矩阵在Δγ＝Δβ＝Δα＝0处进行一阶泰勒展开，得到：

使得：

其中，Δε＝(Δα,Δβ,Δγ)^T是平台在机体坐标系的定姿误差，而Γ¹是把机体坐标系的定姿误差转换到东北天坐标系的误差转换矩阵；

a.对ΔR_l的各元素展开，得到：

b.根据机载平台笛卡尔坐标系到东北天坐标系的真实转换矩阵R_tl及其一阶泰勒展开式，计算得到

各元素：

根据东北天坐标系到ECEF坐标系的转换矩阵R_t，得到：

结合上式以及东北天坐标系到ECEF坐标系的转换矩阵R_t，得到：

X_true＝X_s+R_tR_l(α,β,γ)X₁+ΔX₃＝X_s+R_tR_l(α,β,γ)X₁+ΓΔε

其中，Γ＝R_t·Γ¹是平台定姿误差到ECEF坐标系的转换矩阵，是机体坐标系定姿误差转换到东北天坐标系的误差转换矩阵与旋转变换矩阵的复合。

所述的第二步中：

2.1)令i＝A,B，则平台i东北天坐标系到ECEF坐标系的转换矩阵R_ti为：

其中，(λ_si,l_si,h_si)则表示机载平台i(i＝A,B)中心在大地坐标系的位置，分别表示经度，纬度和高度；

2.2)机载平台i笛卡尔坐标系到东北天坐标系的转换矩阵R_li为：

其中，γ_i,α_i,β_i分别为平台i的偏航角，滚动角和俯仰角。

将步骤2.2)的等式在Δα_A＝Δβ_A＝Δγ_A＝Δα_B＝Δβ_B＝Δγ_B＝0处进行一阶泰勒展开，

得到：

其中，

分别为：

整理得到：

简化后得到：

Γ_fhζ＝Z_fh；

其中，Γ_fh＝[Γ_A -Γ_B]为线性化后的量测矩阵，具体为：

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T为平台三个姿态角的系统偏差，即状态向量，Z_fh＝(X_sA+R_tAR_lA(α_A,β_A,γ_A)X_pA)-(X_sB+R_tBR_lB(α_B,β_B,γ_B)X_pB)为等效量测。

所述第三步实现交替配准的步骤为：

步骤3.1、若两平台A,B对同一个合作目标均有量测，则根据第一步关系式得到：

X_true＝X_sA+R_tAR_lA(α_A,β_A,γ_A)X_1A+Γ_AΔε_A＝X_sB+R_tBR_lB(α_B,β_B,γ_B)X_1B+Γ_BΔε_B；

步骤3.2、将步骤3.1简化为：

其中，

Z_A＝X_true-X_sA-R_tAR_lA(α_A,β_A,γ_A)X_1A，

Z_B＝X_true-X_sB-R_tBR_lB(α_B,β_B,γ_B)X_1B，

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T；

步骤3.3、若两平台A,B中只有平台A对合作目标有量测，则根据第一步关系式得到：

X_true＝X_sA+R_tAR_lA(α_A,β_A,γ_A)X_1A+Γ_AΔε_A；

步骤3.4、将步骤3.3简化，使之能与基于非合作目标实现交替配准：

其中，

Z_A＝X_true-X_sA-R_tAR_lA(α_A,β_A,γ_A)X_1A，

Z_B＝[0 0 0]^T，

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T；

步骤3.5、进行基于合作目标的k时刻卡尔曼滤波，基于合作目标的状态空间为：

步骤3.5.1、对状态进行一步预测和对状态的误差协方差阵进行一步预测：

步骤3.5.2、对状态的增益矩阵的计算：

步骤3.5.3、对状态进行一步更新：

步骤3.5.4、对状态误差协方差阵进行一步更新：

其中，cov(υ_h,k)＝R_h,k+1。

步骤3.6、进行基于非合作目标的k时刻卡尔曼滤波时，

等于

P_kk等于P_k+1|k+1，则基于非合作目标的状态空间为：

步骤3.6.1、对状态进行一步预测和对状态的误差协方差阵进行一步预测；

步骤3.6.2、对状态的增益矩阵的计算：

步骤3.6.3、对状态进行一步更新：

步骤3.6.4、对状态误差协方差阵进行一步更新：

其中，cov(υ_fh,k)＝R_fh,k+1；

步骤3.7、k＝k+1，然后返回步骤3.1，但是此时的

等于步骤3.6.2中的

P_k|k等于步骤3.6.3中的P_k+1|k+1，如此实现所有时刻基于合作目标和基于非合作目标的交替配准。

若是多个合作目标，则先按照步骤3.5对所有的合作目标进行k时刻的参数估计；若是多个非合作目标，则先按照步骤3.6对所有的非合作目标进行k时刻的参数估计。

与现有技术相比，本发明具有如下的有益效果：解决了动平台传感器系统中，传感器量测无偏差而平台姿态角存在偏差的空间配准问题，考虑到地球曲率的影响，本发明基于公共坐标系为地心地固坐标系的三维场景，分析传感器不存在量测偏差的前提下，平台姿态角偏差的估计问题，根据目标类型的不同，设计了基于合作目标为单个或多个的姿态角配准，基于非合作目标为单个或多个的姿态角配准，以及两类目标共存时的姿态角配准，从而得到同时用两类目标的量测信息进行配准，并且能够精确估计姿态角偏差，实现目标的精确跟踪。本发明首先建立以地心地固坐标系为公共坐标系，用于合作目标空间配准的状态空间，并针对一个平台对该合作目标有量测还是两个平台对该合作目标均有量测，然后建立以地心地固坐标系为公共坐标系，用于非合作目标空间配准的状态空间，最后建立基于合作目标配准和基于非合作配准的交替姿态角偏差估计，最终得到的估计精度优于单基于合作目标量测信息进行空间配准的方法以及单基于非合作目标量测信息进行空间配准的方法。

附图说明

图1合作目标、非合作目标及两平台A,B的运动轨迹；

图2平台A姿态角偏差RMSE图：(a)、(b)分别为不同程度放大示意图；

图3平台B姿态角偏差RMSE图：(a)、(b)分别为不同程度放大示意图；

图4本发明方法的整体操作框图；

图5本发明方法的具体流程图。

具体实施方式

下面结合附图对本发明做进一步的详细说明。

本发明动平台传感器系统偏差的估计方法包括以下步骤：

步骤一：基于合作目标建立估计姿态系统偏差的状态空间；

步骤二：基于非合作目标建立估计姿态系统偏差的状态空间；

步骤三：实现基于合作目标和非合作目标的交替配准。

步骤一中基于合作目标建立估计姿态系统偏差的状态空间，具体包括以下步骤：

步骤1.1：传感器载于平台之上，且二者的笛卡尔坐标系重合，考虑地球曲率影响，取地心地固坐标系(ECEF坐标系)作为配准的坐标系。在不考虑传感器量测系统偏差以及姿态角量测噪声的情况下，平台对目标不含偏差的量测位置与合作目标汇报的真实位置在ECEF坐标系下相同，于是可列出等式

X_true＝X_s+R_tR_l(α-Δα,β-Δβ,γ-Δγ)X₁

其中，X_true为目标在ECEF坐标系下的真实位置，X_s为平台在ECEF坐标系下的真实坐标，R_t为东北天坐标系到ECEF坐标系的转换矩阵，R_l为机载平台笛卡尔坐标系到东北天坐标系的转换矩阵，X₁为机载平台(传感器)笛卡尔坐标系下平台对目标的量测，β,γ,α分别是平台俯仰角、偏航角、滚动角的量测值，Δβ,Δγ,Δα分别是平台俯仰角、偏航角、滚动角的系统偏差。

步骤1.1.1：东北天坐标系到ECEF坐标系的转换矩阵R_t为：

此时步骤1.1可表示为：

其中：

此处(x_s,y_s,z_s)表示机载平台中心在ECEF坐标系中的位置，而(λ_s,l_s,h_s)则表示机载平台中心在大地坐标系的位置，分别表示经度，纬度和高度。

步骤1.1.2：机载平台(传感器)笛卡尔坐标系到东北天坐标系的转换矩阵R_l(α,β,γ)为

其中，γ,α,β分别为平台的偏航角，滚动角和俯仰角。

步骤1.1.3：参考步骤1.1.2，机载平台(传感器)笛卡尔坐标系到东北天坐标系的真实转换矩阵R_tl为

其中，Δγ,Δα,Δβ分别为平台的偏航角系统偏差，滚动角系统偏差和俯仰角系统偏差。

步骤1.2：将步骤1.1.3在Δγ＝Δβ＝Δα＝0处进行一阶泰勒展开，得到

使得

其中，Δε＝(Δα,Δβ,Δγ)^T是平台在机体坐标系的定姿误差，而Γ¹是把机体坐标系的定姿误差转换到东北天坐标系的误差转换矩阵。

步骤1.2.1：根据步骤1.2，对ΔR_l的各元素展开，得到

步骤1.2.2：根据步骤1.1.3和步骤1.2，计算得到

各元素，得到

步骤1.3：根据步骤1.1.1，得

步骤1.4：根据步骤1.1.1和步骤1.3，得

X_true＝X_s+R_tR_l(α,β,γ)X₁+ΔX₃＝X_s+R_tR_l(α,β,γ)X₁+ΓΔε

其中Γ＝R_t·Γ¹是平台定姿误差到ECEF坐标系的转换矩阵，是机体坐标系定姿误差转换到东北天坐标系的误差转换矩阵与旋转变换矩阵的复合。

步骤二中基于非合作目标建立估计姿态系统偏差的状态空间，具体步骤如下：

步骤2.1：传感器载于平台之上，且二者的笛卡尔坐标系重合，考虑地球曲率影响，取地心地固坐标系(ECEF坐标系)作为配准的坐标系。在目标是运动的非合作目标的前提下对两个平台进行配准。配准的主要思路是，在不考虑传感器的系统偏差及平台的姿态角量测噪声的情况下，两平台A,B在同一时刻，对目标的不含偏差量测相同，则可列出列出等式

其中，X_sA,X_sB分别为平台A,B在ECEF坐标系下的坐标，R_tA,R_tB分别为平台A,B东北天坐标系到ECEF坐标系的转换矩阵，R_lA,R_lB分别为平台A,B机载平台(传感器)笛卡尔坐标系到东北天坐标系的转换矩阵，X_pA,X_pB分别为平台A,B在机载平台(传感器)笛卡尔坐标系下传感器对目标的量测，(α_A,β_A,γ_A)和(α_B,β_B,γ_B)分别是平台A,B的滚动角、俯仰角、偏航角的量测值，(Δα_A,Δβ_A,Δγ_A)和(Δα_B,Δβ_B,Δγ_B)分别是平台A,B相应姿态角的系统偏差。

步骤2.1.1：平台i(i＝A,B)东北天坐标系到ECEF坐标系的转换矩阵R_ti为

其中，(λ_si,l_si,h_si)则表示机载平台i(i＝A,B)中心在大地坐标系的位置，分别表示经度，纬度和高度。

步骤2.1.2：机载平台(传感器)i(i＝A,B)笛卡尔坐标系到东北天坐标系的转换矩阵R_li为

其中，γ_i,α_i,β_i分别为平台i的偏航角，滚动角和俯仰角。

步骤2.2：由于平台的姿态偏差相对较小，参考步骤2.1.1和步骤2.1.2，将步骤2.1等式在Δα_A＝Δβ_A＝Δγ_A＝Δα_B＝Δβ_B＝Δγ_B＝0处进行一阶泰勒展开，得

其中，

分别为

步骤2.3：将步骤2.2进行整理，得

步骤2.4：将步骤2.3进行简化，得

Γ_fhζ＝Z_fh

其中，Γ_fh＝[Γ_A -Γ_B]为线性化后的量测矩阵，具体为

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T为平台三个姿态角的系统偏差，即状态向量，

Z_fh＝(X_sA+R_tAR_lA(α_A,β_A,γ_A)X_pA)-(X_sB+R_tBR_lB(α_B,β_B,γ_B)X_pB)为等效量测。

步骤三中，对基于合作目标的量测信息和基于非合作目标的量测信息用卡尔曼滤波交替估计平台的姿态角偏差，具体步骤如下：

步骤3.1：若两平台(传感器)A,B对同一个合作目标均有量测，则根据步骤一的建模过程，可分别得到

X_true＝X_sA+R_tAR_lA(α_A,β_A,γ_A)X_1A+Γ_AΔε_A＝X_sB+R_tBR_lB(α_B,β_B,γ_B)X_1B+Γ_BΔε_B

步骤3.2：步骤3.1可简化为

其中，

Z_A＝X_true-X_sA-R_tAR_lA(α_A,β_A,γ_A)X_1A

Z_B＝X_true-X_sB-R_tBR_lB(α_B,β_B,γ_B)X_1B

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T

步骤3.3：若两平台(传感器)A,B中只有平台(传感器)A对合作目标有量测，则根据步骤一的建模过程，可得到

X_true＝X_sA+R_tAR_lA(α_A,β_A,γ_A)X_1A+Γ_AΔε_A

步骤3.4：但是为了能与基于非合作目标实现交替配准，步骤3.3需简化为

其中，

Z_A＝X_true-X_sA-R_tAR_lA(α_A,β_A,γ_A)X_1A

Z_B＝[0 0 0]^T

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T

步骤3.5：步骤一为了简便起见，忽略了平台传感器的量测噪声，下面在进行卡尔曼滤波的时候将噪声考虑在内。依据步骤3.1至3.4，进行基于合作目标的k时刻的卡尔曼滤波，基于合作目标的状态空间为

步骤3.5.1：对状态进行一步预测和对状态的误差协方差阵进行一步预测

步骤3.5.2：对状态的增益矩阵的计算

步骤3.5.3：对状态进行一步更新

步骤3.5.4：对状态误差协方差阵进行一步更新

其中，cov(υ_h,k)＝R_h,k+1。

步骤3.6：步骤二为了简便起见，忽略了平台传感器的量测噪声，下面在进行卡尔曼滤波的时候将平台姿态角量测噪声考虑在内。依据步骤二，进行基于非合作目标的k时刻的卡尔曼滤波，但是此时的

等于步骤3.5.2中的

P_k|k等于步骤3.5.3中的P_k+1|k+1，基于非合作目标的状态空间为

步骤3.6.1：对状态进行一步预测和对状态的误差协方差阵进行一步预测

步骤3.6.2：对状态的增益矩阵的计算

步骤3.6.3：对状态进行一步更新

步骤3.6.4：对状态误差协方差阵进行一步更新

其中，cov(υ_fh,k)＝R_fh,k+1

步骤3.7：k＝k+1，然后返回步骤3.1，但是此时的

等于步骤3.6.2中的

P_k|k等于步骤3.6.3中的P_k+1|k+1。如此实现所有时刻的基于合作目标和基于非合作目标的交替配准。当然，若是多个合作目标，则先按照步骤3.5对所有的合作目标进行k时刻的参数估计，同样的，若是多个非合作目标，则先按照步骤3.6对所有的非合作目标进行k时刻的参数估计，具体参见图5。

参见图1-3，本发明利用PC平台仿真来实现对算法的验证。仿真软件为Windows 7旗舰版，MATLAB R2014a版本。仿真场景设置为：两平台对一个合作目标和一个非合作目标进行观测，其中两平台对该合作目标均有量测，平台A和B在大地坐标系下的初始位置(纬度，经度，高度)分别为(33.9°,108.2°,6000m)和(33.9°,107.9°,6000m)，在纬度、经度、高度的速度向量均为(-0.12km/s,-0.12km/s,0)，平台A和B的姿态角真值分别为(25°,10°,20°)和(30°,15°,25°)，姿态角系统偏差分别为(0.5°,-0.95°,0.55°)和(0.45°,-0.7°,0.4°)，所有的角度量测噪声标准差均为0.1°，合作目标和非合作目标在在大地坐标系下的初始位置(纬度，经度，高度)分别为(33°,107°,6000m)和(33.4°^,107.2°,6000m)，在纬度、经度、高度的速度向量分别为(0.1km/s,0.1km/s,0)和(0.1km/s,0,0)，采样周期为1s，整个仿真时间为1000s，蒙特卡洛仿真次数为100次。图1为两个目标和两个平台在大地坐标系下的运动轨迹，从图2(a)和图2(b)可以看出利用平台A对合作目标和非合作目标的量测信息可以得到最精确的姿态角偏差估计，从图3(a)和图3(b)可以看出利用平台B对合作目标和非合作目标的量测信息可以得到最精确的姿态角偏差估计，精确的姿态角偏差估计可以实现对目标更准确的跟踪。

Claims (7)

1.一种动平台传感器系统偏差的估计方法，其特征在于，包括以下步骤：

第一步、基于合作目标建立估计姿态系统偏差的状态空间；

将传感器载于平台上，使二者的笛卡尔坐标系重合，取ECEF坐标系为配准的坐标系，得到：X_true＝X_s+R_tR_l(α-Δα,β-Δβ,γ-Δγ)X₁；式中，X_true为目标在ECEF坐标系下的真实位置，X_s为平台在ECEF坐标系下的真实坐标，R_t为东北天坐标系到ECEF坐标系的转换矩阵，R_l为机载平台笛卡尔坐标系到东北天坐标系的转换矩阵，X₁为机载平台笛卡尔坐标系下平台对目标的量测，β,γ,α分别是平台俯仰角、偏航角、滚动角的量测值，Δβ,Δγ,Δα分别是平台俯仰角、偏航角、滚动角的系统偏差；

第二步、基于非合作目标建立估计姿态系统偏差的状态空间；

将传感器载于平台上，使二者的笛卡尔坐标系重合，取ECEF坐标系为配准的坐标系，对运动的非合作目标，在不考虑传感器的系统偏差及平台的姿态角量测噪声的情况下，两平台A,B在同一时刻，对目标的不含偏差量测相同，得到：

X_sA+R_tAR_lA(α_A-Δα_A,β_A-Δβ_A,γ_A-Δγ_A)X_pA

＝X_sB+R_tBR_lB(α_B-Δα_B,β_B-Δβ_B,γ_B-Δγ_B)X_pB

式中，X_sA,X_sB分别为平台A,B在ECEF坐标系下的坐标，R_tA,R_tB分别为平台A,B东北天坐标系到ECEF坐标系的转换矩阵，R_lA,R_lB分别为平台A,B机载平台笛卡尔坐标系到东北天坐标系的转换矩阵，X_pA,X_pB分别为平台A,B在机载平台笛卡尔坐标系下传感器对目标的量测，(α_A,β_A,γ_A)和(α_B,β_B,γ_B)分别是平台A,B的滚动角、俯仰角、偏航角的量测值，(Δα_A,Δβ_A,Δγ_A)和(Δα_B,Δβ_B,Δγ_B)分别是平台A,B相应姿态角的系统偏差；

第三步、实现基于合作目标和非合作目标的交替配准，实现交替配准的步骤为：

步骤3.1、若两平台A,B对同一个合作目标均有量测，则根据第一步关系式得到：

X_true＝X_sA+R_tAR_lA(α_A,β_A,γ_A)X_1A+Γ_AΔε_A＝X_sB+R_tBR_lB(α_B,β_B,γ_B)X_1B+Γ_BΔε_B；

步骤3.2、将步骤3.1简化为：

其中，

Z_A＝X_true-X_sA-R_tAR_lA(α_A,β_A,γ_A)X_1A，

Z_B＝X_true-X_sB-R_tBR_lB(α_B,β_B,γ_B)X_1B，

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T；

Γ_A，Γ_B分别为平台A，B定姿误差到ECEF坐标系的转换矩阵；

步骤3.3、若两平台A,B中只有平台A对合作目标有量测，则根据第一步关系式得到：

X_true＝X_sA+R_tAR_lA(α_A,β_A,γ_A)X_1A+Γ_AΔε_A；

步骤3.4、将步骤3.3简化，使之能与基于非合作目标实现交替配准：

其中，

Z_A＝X_true-X_sA-R_tAR_lA(α_A,β_A,γ_A)X_1A，

Z_B＝[0 0 0]^T，

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T；

步骤3.5、进行基于合作目标的k时刻卡尔曼滤波，基于合作目标的状态空间为：

步骤3.5.1、对状态进行一步预测和对状态的误差协方差阵进行一步预测：

步骤3.5.2、对状态的增益矩阵的计算：

步骤3.5.3、对状态进行一步更新：

步骤3.5.4、对状态误差协方差阵进行一步更新：

其中，cov(υ_h,k)＝R_h,k+1；

步骤3.6、进行基于非合作目标的k时刻卡尔曼滤波时，

等于

P_k|k等于P_k+1|k+1，则基于非合作目标的状态空间为：

步骤3.6.1、对状态进行一步预测和对状态的误差协方差阵进行一步预测；

步骤3.6.2、对状态的增益矩阵的计算：

步骤3.6.3、对状态进行一步更新：

步骤3.6.4、对状态误差协方差阵进行一步更新：

其中，cov(υ_fh,k)＝R_fh,k+1；

Γ_fh为线性化后的量测矩阵；

步骤3.7、k＝k+1，然后返回步骤3.1，但是此时的

等于步骤3.6.2中的

P_k|k等于步骤3.6.3中的P_k+1|k+1，如此实现所有时刻基于合作目标和基于非合作目标的交替配准。

2.根据权利要求1所述动平台传感器系统偏差的估计方法，其特征在于，所述的第一步：

1.1)得到东北天坐标系到ECEF坐标系的转换矩阵R_t为：

得到：

其中：

此处(x_s,y_s,z_s)表示机载平台中心在ECEF坐标系中的位置，而(λ_s,l_s,h_s)则表示机载平台中心在大地坐标系的位置，分别表示经度，纬度和高度；

1.2)得到机载平台笛卡尔坐标系到东北天坐标系的转换矩阵R_l(α,β,γ)为：

其中，γ,α,β分别为平台的偏航角，滚动角和俯仰角；

1.3)得到机载平台笛卡尔坐标系到东北天坐标系的真实转换矩阵R_tl为：

其中，Δγ,Δα,Δβ分别为平台的偏航角系统偏差，滚动角系统偏差和俯仰角系统偏差。

3.根据权利要求2所述动平台传感器系统偏差的估计方法，其特征在于，将步骤1.3)得到的矩阵在Δγ＝Δβ＝Δα＝0处进行一阶泰勒展开，得到：

使得：

其中，Δε＝(Δα,Δβ,Δγ)^T是平台在机体坐标系的定姿误差，而Γ¹是把机体坐标系的定姿误差转换到东北天坐标系的误差转换矩阵；

a.对ΔR_l的各元素展开，得到：

b.根据机载平台笛卡尔坐标系到东北天坐标系的真实转换矩阵R_tl及其一阶泰勒展开式，计算得到

各元素：

4.根据权利要求3所述动平台传感器系统偏差的估计方法，其特征在于，根据东北天坐标系到ECEF坐标系的转换矩阵R_t，得到：

结合上式以及东北天坐标系到ECEF坐标系的转换矩阵R_t，得到：

X_true＝X_s+R_tR_l(α,β,γ)X₁+ΔX₃＝X_s+R_tR_l(α,β,γ)X₁+ΓΔε

其中，Γ＝R_t·Γ¹是平台定姿误差到ECEF坐标系的转换矩阵，是机体坐标系定姿误差转换到东北天坐标系的误差转换矩阵与旋转变换矩阵的复合。

5.根据权利要求1所述动平台传感器系统偏差的估计方法，其特征在于，所述的第二步：

2.1)令i＝A,B，则平台i东北天坐标系到ECEF坐标系的转换矩阵R_ti为：

其中，(λ_si,l_si,h_si)则表示机载平台i(i＝A,B)中心在大地坐标系的位置，分别表示经度，纬度和高度；

2.2)机载平台i笛卡尔坐标系到东北天坐标系的转换矩阵R_li为：

其中，γ_i,α_i,β_i分别为平台i的偏航角，滚动角和俯仰角。

6.根据权利要求5所述动平台传感器系统偏差的估计方法，其特征在于，将步骤2.2)的等式在Δα_A＝Δβ_A＝Δγ_A＝Δα_B＝Δβ_B＝Δγ_B＝0处进行一阶泰勒展开，得到：

其中，

分别为：

整理得到：

简化后得到：

Γ_fhζ＝Z_fh；

其中，Γ_fh＝[Γ_A -Γ_B]为线性化后的量测矩阵，具体为：

ζ＝[Δα_A Δβ_A Δγ_A Δα_B Δβ_B Δγ_B]^T为平台三个姿态角的系统偏差，即状态向量，

Z_fh＝(X_sA+R_tAR_lA(α_A,β_A,γ_A)X_pA)-(X_sB+R_tBR_lB(α_B,β_B,γ_B)X_pB)为等效量测。

7.根据权利要求1所述动平台传感器系统偏差的估计方法，其特征在于，若是多个合作目标，则先按照步骤3.5对所有的合作目标进行k时刻的参数估计；若是多个非合作目标，则先按照步骤3.6对所有的非合作目标进行k时刻的参数估计。

Images