[发明专利]系绳约束下的空间三体系统多传感器精确定位方法

权利要求书

1.一种系绳约束下的空间三体系统多传感器精确定位方法，其特征在于：包括以下步骤：

步骤1：构造空间三体系统的动力学模型：

建立的空间三体系统的动力学模型为

其中X＝[r₁ r₂ r₃]^T，r_i＝[x_i y_i z_i]^T(i＝1,2,3)为空间旋转绳系卫星系统中三颗卫星质点在地心惯性坐标系中的位置矢量，μ为引力常数；所述空间旋转绳系卫星系统由三根系绳和三颗卫星组成，并且卫星通过系绳连接起来形成闭合的平面三角形状；

步骤2：基于细绳约束条件建立观测模型：

根据空间三体系统在运动的过程中系绳长度保持不变，建立观测模型为

Z＝H(X)

其中H(X)＝[(r₁-r₂)² (r₂-r₃)² (r₁-r₃)²]^T，l₁、l₂、l₃为三根系绳长度；

步骤3：根据步骤1和步骤2得到的空间三体系统的动力学模型和观测模型，建立空间三体系统的状态方程和观测方程如下：

其中W(k)为过程噪声，其为均值为0，方差为Q的高斯白噪声；V(k+1)为观测噪声，其为均值为0，方差为R的高斯白噪声；

根据空间三体系统的状态方程和观测方程，采用扩展卡尔曼滤波算法对空间三体系统中每个卫星上的星载全球定位系统接收机的数据进行融合，实现对空间三体系统中的三个卫星位置精确定位，具体步骤为：

步骤3.1：初始化初始状态X(0)、Z(0)、协方差矩阵P(0)；

步骤3.2：确定状态方程

步骤3.3：确定观测方程Z(k+1)＝H(X(k+1))+V(k+1)；

步骤3.4：一阶线性化状态方程，将一阶线性化处理后，得到X(k+1|k)＝f[k,X(k)]+W(k)，求解状态转移矩阵

步骤3.5：一阶线性化观测方程，将Z(k+1)＝H(X(k+1))+V(k+1)一阶线性化处理后，得到Z(k+1)＝h[k+1,X(k+1)]+V(k+1)，求解观测矩阵

步骤3.6：求协方差矩阵P(k+1|k)＝Φ(k+1)P(k|k)Φ^T(k+1)+Q；

步骤3.7：求卡尔曼滤波增益K(k+1)＝P(k+1|k)Θ^T(k)/[Θ(k)P(k+1|k)Θ^T(k)+R]；

步骤3.8：求状态更新X(k+1)＝X(k+1|k)+K(k+1)[Z(k+1)-Z(k)]；

步骤3.9：协方差更新P(k+1)＝[I_n-K(k+1)Θ(k+1)]P(k+1|k)，其中矩阵I_n为单位矩阵。