[发明专利]一种降低星敏感器和陀螺联合滤波计算量的改进方法

摘要

本发明公开了一种降低星敏感器和陀螺联合滤波计算量的改进方法，其包含：步骤1，建立星敏感器与陀螺联合系统的状态方程与观测方程；步骤2，计算状态量预测值步骤3，计算系统误差协方差阵的预测值P(k|k‑1)；步骤4，计算滤波增益系数Ki；步骤5，计算系统误差协方差阵预测值P(k|k‑1)的第i次迭代值Pi；步骤6，计算状态量预测值的第i次迭代值步骤7，单个节拍内迭代计算步骤(4)～步骤(6)，迭代次数为量测值y(k)的维数l；步骤8，每个节拍重复步骤(2)～步骤(7)，更新状态量本发明基于星敏感器量测噪声方差阵为对角矩阵的特点，将量测值视为独立的标量进行滤波增益系数的迭代求解，极大地降低了星敏感器与陀螺联合滤波的计算量，星载软件容易实现。

主权项

1.一种降低星敏感器和陀螺联合滤波计算量的改进方法，其特征在于，该方法包含如下步骤：步骤1，建立星敏感器与陀螺联合系统的状态方程与观测方程，其公式如下：其中，x(k)、x(k‑1)分别为k、k‑1时刻系统的状态量阵；A(k‑1)为系统状态一步转移阵；B(k‑1)为系统噪声驱动阵；w(k‑1)为系统噪声方差阵；C(k)为量测阵；v_q(k)为量测噪声阵；y(k)为量测值。步骤2，计算状态量预测值其公式如下：其中，为k‑1时刻状态量的估计值阵。步骤3，计算系统误差协方差阵的预测值P(k|k‑1)，其公式如下：P(k|k‑1)＝A(k‑1)P(k‑1)A(k‑1)^T+B(k‑1)QB(k‑1)^T     (3)其中，P(k‑1)为k‑1时刻系统误差协方差阵的估计值；Q为系统噪声方差阵。步骤4，计算滤波增益系数K_i，其公式如下：K_i＝P_i‑1C_i^T/(C_iP_i‑1C_i^T+R_i,i)                (4)其中，P_i‑1为系统误差协方差阵预测值P(k|k‑1)的第i‑1次迭代值，P₀等于步骤(4)计算的P(k|k‑1)；C_i为C(k)的第i行数值；R_i,i为量测噪声方差阵R的第i行第i列的数值。步骤5，计算系统误差协方差阵预测值P(k|k‑1)的第i次迭代值P_i，其公式如下：P_i＝P_i‑1‑P_i‑1K_iC_i                     (5)步骤6，计算状态量预测值的第i次迭代值其公式如下：其中，y_i为量测值y(k)的第i个数值；等于步骤(3)计算的步骤7，单个节拍内迭代计算步骤(4)～步骤(6)，迭代次数为量测值y(k)的维数l。之后计算得到k时刻系统误差协方差阵估计值P(k)和状态量估计值其公式如下：步骤8，每个节拍重复步骤(2)～步骤(7)，迭代计算状态量