一种基于星敏感器和陀螺的高精度卫星姿态确定方法

摘要

本发明公开了一种基于星敏感器和陀螺的高精度卫星姿态确定方法，包括以下几个步骤，步骤一：建立卫星姿态确定系统的状态方程；步骤二：建立卫星姿态确定系统的测量方程；步骤三：利用预测滤波在线实时估计模型误差；步骤四：对补偿后的模型利用二阶插值滤波进行状态估计，得到卫星的姿态。本发明采用了预测滤波在线实时估计模型误差并修正系统模型，克服了传统估计过程中将误差处理为零均值白噪声的缺点；而且可以处理任何非线性系统和噪声情况，获得更高精度的估计结果，适用于高精度姿态确定领域。

说明书

技术领域

本发明涉及一种基于星敏感器和陀螺的高精度卫星姿态确定方法，属于空间飞行器高精度姿态确定技术领域。

背景技术

飞行器姿态确定系统是飞行器姿态控制系统中的重要组成部分，其精度和可靠性是飞行器长期正常运行的重要保证和前提。该系统主要由姿态敏感器和相应姿态确定算法组成，为了克服参考矢量的不确定性对姿态确定精度的影响，状态估计的方法在高精度姿态确定场合得到了广泛的应用。

目前常用的方法是扩展卡尔曼滤波(Extended Kalman Filter，EKF)，EKF具有收敛速度快，算法简单，可以实时解算等优点。但是EKF因为采用了对非线性模型Taylor展开近似的方法，在非线性较强时估计偏差较大。并且EKF需要计算雅克比矩阵，要求函数必须可微，限制了其使用范围。此外，EKF将系统噪声处理为零均值白噪声，这就要求建模必须十分精确，若实际系统中的噪声不是高斯白噪声，而被作为白噪声来处理，则将导致估计精度严重下降。非线性预测滤波(Nonlinear Predict Filter，NPF)考虑了系统模型误差或未知输入的影响，具有对模型误差的估计能力，与EKF等其他非线性滤波方法相比，预测滤波能够在线估计出未知模型误差，克服了将模型误差和系统噪声设为高斯白噪声的局限，在飞行器姿态估计领域中得到了应用。但是预测滤波收敛速度相对较慢，收敛时估计出的模型误差的不准确会对滤波收敛性和精度造成影响。

发明内容

本发明的目的是为了解决飞行器高精度姿态确定方面现有技术的不足，提出一种基于星敏感器和陀螺的高精度卫星姿态确定方法。

本发明的一种基于星敏感器和陀螺的高精度卫星姿态确定方法，包括以下几个步骤：

步骤一：建立卫星姿态确定系统的状态方程；

步骤二：建立卫星姿态确定系统的测量方程；

步骤三：利用预测滤波在线实时估计模型误差；

步骤四：对补偿后的模型利用二阶插值滤波进行状态估计，得到卫星的姿态。

本发明的优点在于：

(1)采用了预测滤波在线实时估计模型误差并修正系统模型，克服了传统估计过程中将误差处理为零均值白噪声的缺点。

(2)采用了二阶插值滤波算法，是一种比EKF精度更高的多项式插值滤波算法，且不需要计算偏导数，适用范围更广；

(3)本发明采用了预测滤波估计并补偿系统模型误差，可以处理任何非线性系统和噪声情况，获得更高精度的估计结果，适用于高精度姿态确定领域。

附图说明

图1是本发明的方法流程图；

图2a是本发明的方法与EKF方法仿真结果的俯仰角误差曲线对比图；

图2b是本发明的方法与EKF方法仿真结果的偏航角误差曲线对比图；

图2c是本发明的方法与EKF方法仿真结果的滚转角误差曲线对比图；

图3a是本发明的方法与二阶插值滤波方法仿真结果的俯仰角误差曲线对比图；

图3b是本发明的方法与二阶插值滤波方法仿真结果的偏航角误差曲线对比图；

图3c是本发明的方法与二阶插值滤波方法仿真结果的滚转角误差曲线对比图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明是一种基于星光和陀螺的高精度卫星姿态确定方法，利用陀螺和星敏感器作为姿态敏感器，利用预测滤波的思想实时估计出卫星姿态确定系统的模型误差并实时进行补偿，再利用精度更高的二阶插值滤波估计出卫星本体坐标系相对于惯性坐标系的姿态四元数，然后经过解算得到卫星姿态，流程如图1所示，包括以下几个步骤：

步骤一：建立卫星姿态确定系统的状态方程；

用姿态动力学方程与运动学方程一起来描述卫星的姿态变化，姿态动力学方程可从刚体的动量矩公式和定理导出，即欧拉-牛顿法：根据刚体动量矩定理，卫星的姿态动力学方程为：

$J\stackrel{·}{\omega }+\omega \times \mathrm{J\omega }=G_{\mathrm{gb}}+G_{\mathrm{mb}}+G_{\mathrm{ab}}+G_{\mathrm{sb}}---\left(1\right)$

式中，ω＝[ω_x，ω_y，ω_z]^T为卫星本体坐标系对惯性坐标系的转动角速度，ω_x、ω_y、ω_z为卫星本体坐标系对惯性坐标系在卫星本体坐标系下沿x，y，z三个轴的分量；J为卫星的惯性张量矩阵；G_mb为地磁力矩；G_ab为气动力矩；G_sb为太阳光压力矩；G_gb为重力梯度力矩；

G_gb具体为：

$G_{\mathrm{gb}}=\frac{3\mu }{r^5}{R}_b\times {\mathrm{JR}}_b---\left(2\right)$

式中，μ为地心引力常数；r为卫星的地心距；R_b为卫星在卫星本体坐标系下的位置矢量，具体为：

$R_b=C_i^b{R}_i---\left(3\right)$

式中，R_i为卫星在惯性坐标系下的位置矢量，为从惯性坐标系到卫星本体坐标系的转换矩阵，具体为：

地心距真近点角偏近点角平近点角M＝n(t-t0)；n为卫星的平运动速度；t₀为卫星到达近地点的时间；t为时间；为地心轨道坐标系到惯性坐标系的转换矩阵，具体为：

$C_o^i=\left(\begin{array}{ccc}\cos {\Omega }_o\cos w_o-\sin w_o\cos i_o\sin {\Omega }_o& -\cos {\Omega }_o\sin w_o-\cos w_o\cos i_o\sin {\Omega }_o& \sin {\Omega }_o\sin i_o\\ \sin {\Omega }_o\cos w_o+\sin w_o\cos i_o\cos {\Omega }_o& -\sin {\Omega }_o\sin w_o+\cos w_o\cos i_o\cos {\Omega }_o& -\cos {\Omega }_o\sin i_o\\ \sin w_o\sin i_o& \sin w_o\cos i_o& \cos i_o\end{array}\right)$

其中，Ω_o为卫星轨道升交点赤经；i_o为卫星轨道倾角；w_o为卫星轨道近地点幅角。

因为G_gb比地磁力矩G_mb、气动力矩G_ab和太阳光压力矩G_sb对卫星姿态确定系统的影响大得多，所以将G_mb、G_ab和G_sb用合力矩用d_G表示，作为未建模的模型误差，则式(1)的卫星姿态动力学方程写为：

$\stackrel{·}{\omega }=J^{-1}(-\omega \times \mathrm{J\omega }+G_{\mathrm{gb}})+J^{-1}{d}_G---\left(4\right)$

姿态运动学方程是姿态参数在姿态机动过程中变化的方程，由于四元数表示的姿态运动学方程为线性微分方程，不含三角函数，无奇点问题，所以采用四元数来描述的卫星姿态运动学方程如下：

$\stackrel{·}{q}=\frac{1}{2}\Omega \left(\omega \right)q=\frac{1}{2}\Xi \left(q\right)\omega ---\left(5\right)$

式中，为卫星本体坐标系相对于惯性坐标系的姿态四元数；q₁₃＝[q₁q₂q₃]^T，I_3×3为单位阵；[ω×]、[q₁₃×]分别表示由向量ω、q₁₃的分量构成的反对称矩阵，

由式(4)和式(5)构成卫星姿态确定系统的状态方程为：

$\stackrel{·}{x}=\left(\begin{array}{c}{J}^{-1}(-\omega \times \mathrm{J\omega }+G_{\mathrm{gb}})\\ \frac{1}{2}\Xi \left(q\right)\omega \end{array}\right)+G·d_G+W---\left(6\right)$

式中：x为状态向量，x＝[ω^T，q^T]^T；G为误差扰动矩阵，系统噪声w(k)～(0，Q(k))，即w(k)服从均值为0、方差为Q(k)的高斯分布。

将式(6)简记为：

$\stackrel{·}{x}=f(x,d_G,w)---\left(7\right)$

步骤二：建立卫星姿态确定系统的测量方程；

本发明中采用三轴陀螺和星敏感器作为姿态敏感器。

1)陀螺；

陀螺的测量方程为：

$g_1\left(x\left(k\right)\right)=\left(\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right)+\left(\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right)=g_{\omega }\left(x\left(k\right)\right){+v}_{\omega }\left(k\right)---\left(8\right)$

式中，g₁为陀螺的测量输出，记ωi(i＝x，y，z)为卫星本体坐标系对惯性坐标系的转动角速度，v_i(i＝x，y，z)为陀螺测量的高斯白噪声。

(2)星敏感器；

本发明采用两个星敏感器进行测量，在惯性坐标系中两个星敏感的主光轴的单位方向矢量分别是l_i1和l_i2，则星敏感器的测量方程为：

$\left(\begin{array}{c}{g}_2=C_i^b{l}_{i1}+v_{s1}=g_{s1}\left(x\left(k\right)\right)+v_{s1}\\ g_3=C_i^b{l}_{i2}+v_{s2}=g_{s2}\left(x\left(k\right)\right)+v_{s2}\end{array}\right)---\left(9\right)$

式中，v_s1、v_s2是星敏感器测量的噪声，这里主要考虑为高斯白噪声；g₁、g₂分别是两个星敏感器的测量输出；记是由惯性坐标系到卫星本体坐标系的转换矩阵，可用姿态四元数表示，具体为：

$C_i^b=\left(\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right)---\left(10\right)$

(3)建立卫星姿态确定系统的观测方程为：

$y\left(k\right)=\left(\begin{array}{c}{g}_{\omega }\left(x\left(k\right)\right)\\ g_{s1}\left(x\left(k\right)\right)\\ g_{s2}\left(x\left(k\right)\right)\end{array}\right)+\left(\begin{array}{c}{v}_{\omega }\left(k\right)\\ v_{s1}\left(k\right)\\ v_{s2}\left(k\right)\end{array}\right)---\left(11\right)$

将式(11)简记为：

y(k)＝g(x(k)，v(k))    (12)

其中，即v(k)服从均值为0，方差为R(k)的高斯分布，并且w(k)，v(k)相互独立。

步骤三：利用预测滤波在线实时估计模型误差；

模型误差的估计式为：

${\hat{d}}_G\left(k\right)=-{\left({\left\{\Lambda \left(T\right)U\right[\hat{x}\left(k\right)\left]\right\}}^T{R}^{-1}\right\{\Lambda \left(T\right)U\left[\hat{x}\left(k\right)\right]\}+W)}^{-1}{\left\{\Lambda \left(T\right)U\right[\hat{x}\left(k\right)\left]\right\}}^T{R}^{-1}$(13)

$x\{\hat{y}\left(k\right)+z[\hat{x}\left(k\right),T]-y(k+1)\}$

式中：为陀螺和两个星敏感器的输出估计，T为滤波周期，U为灵敏度矩阵，Λ(T)为对角矩阵，为一个列向量。W为加权矩阵，预先设定数值。

其中，具体为：

$\hat{y}\left(k\right)=g\left[\hat{x}\left(k\right)\bar{v}\left(k\right)\right]---\left(14\right)$

式中：为k时刻的状态量的估计值，表示v(k)的均值。

灵敏度矩阵U为：

$U=\left(\begin{array}{c}{L}_G\left[L_f^0\left(g_w\right)\right]\\ L_G\left[L_f^1\left(g_{s1}\right)\right]\\ L_G\left[L_f^1\left(g_{s2}\right)\right]\end{array}\right),---\left(15\right)$

相关的李导数的具体为：

$L_G\left[L_f^0\left(g_w\right)\right]=J^{-1}$

$L_G\left[{L^1}_f\left(g_{s1}\right)\right]=\frac{\partial {L^1}_f\left(g_{s1}\right)}{\partial \hat{x}}·G=-{\Xi }^T\left(\hat{q}\right)\Gamma \left(l_{i1}\right)\Xi \left(\hat{q}\right)J^{-1}$

$L_G\left[{L^1}_f\left(g_{s2}\right)\right]=\frac{\partial {L^1}_f\left(g_{s2}\right)}{\partial \hat{x}}·G=-{\Xi }^T\left(\hat{q}\right)\Gamma \left(l_{i2}\right)\Xi \left(\hat{q}\right)J^{-1}$

对角矩阵Λ(T)为：

$\Lambda \left(T\right)=\left(\begin{array}{ccc}{\Lambda }_w& 0& 0\\ 0& {\Lambda }_{s1}& 0\\ 0& 0& {\Lambda }_{s2}\end{array}\right)---\left(16\right)$

其中：Λ_w＝T·I_3×3，${\Lambda }_{s1}=\frac{T^2}{2}·I_{3\times 3},$${\Lambda }_{s2}=\frac{T^2}{2}·I_{3\times 3},;$

列向量为：

$z[\hat{x}\left(k\right),T]=\left(\begin{array}{c}{z}_{\omega }\\ z_{s1}\\ z_{s2}\end{array}\right)=\left(\begin{array}{c}T·{L^1}_f\left(g_{\omega }\right)\\ T·{L^1}_f\left(g_{s1}\right)+\frac{T^2}{2}{L^2}_f\left(g_{s1}\right)\\ T·{L^1}_f\left(g_{s2}\right)+\frac{T^2}{2}·{L^2}_f\left(g_{s2}\right)\end{array}\right)---\left(17\right)$

上式中各阶李导数的为：

${L^1}_f\left(g_{\omega }\right)=J^{-1}[-\hat{\omega }\times J\hat{\omega }+G_{\mathrm{gb}}]$

${L^1}_f\left(g_{s1}\right)=-{\Xi }^T\left(\hat{q}\right)\Gamma \left(l_{i1}\right)\Xi \left(\hat{q}\right)\hat{\omega }$

${L^2}_f\left(g_{s1}\right)=[\hat{\omega }\times ]·{\Xi }^T\left(\hat{q}\right)\Gamma \left(l_{i1}\right)\Xi \left(\hat{q}\right)\hat{\omega }-{\Xi }^T\left(\hat{q}\right)\Gamma \left(l_{i1}\right)\Xi \left(\hat{q}\right)\times J^{-1}[-\hat{\omega }\times J\hat{\omega }+G_{\mathrm{gb}}]$

${L^1}_f\left(g_{s2}\right)=-{\Xi }^T\left(\hat{q}\right)\Gamma \left(l_{i2}\right)\Xi \left(\hat{q}\right)\hat{\omega }$

${L^2}_f\left(g_{s2}\right)=\left[\hat{\omega }\times \right]·{\Xi }^T\left(\hat{q}\right)\Gamma \left(l_{i2}\right)\Xi \left(\hat{q}\right)\hat{\omega }-{\Xi }^T\left(\hat{q}\right)\Gamma \left(l_{i2}\right)\Xi \left(\hat{q}\right)\times J^{-1}[-\hat{\omega }\times J\hat{\omega }+G_{\mathrm{gb}}]$

其中：$\Gamma \left(a\right)=\left(\begin{array}{cc}-\left[a\right]\times & -a\\ a^T& 0\end{array}\right).$

最后，根据式(13)得到模型误差的估计值

步骤四：对补偿后的模型利用二阶插值滤波进行状态估计，得到卫星的姿态；

插值滤波是利用Stirling插值公式将非线性函数进行近似，是一种基于最小均方误差准则的估计方法。

具体为：

将步骤三中得到的模型误差代入状态方程(7)进行补偿，将补偿后的卫星姿态确定系统模型的状态方程和测量方程写成离散的非线性形式，为：

x_k+1＝f(x_k，d_G，w_k)

                   (18)

y_k＝g(x_k，v_k)

设滤波值的误差方差阵的平方根为即是的Cholesky分解，模型误差方差阵Q的平方根为S_w，测量噪声的误差方差阵R的平方根为S_v，状态预测误差的方差阵的平方根为即：

$Q=S_w{S}_w^T,$$R=S_v{S}_v^T,$(19)

$\bar{P}={\bar{S}}_x{\stackrel{-T}{S}}_x,$$\hat{P}={\hat{S}}_x{\hat{S}}_x^T.$

状态及其误差方差阵的一步预测：

${\bar{x}}_{k+1}=\frac{h^2-n_x-n_w}{h^2}f({\hat{x}}_k,{\hat{d}}_G,{\bar{w}}_k)$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_x}{\Sigma }}(f({\hat{x}}_k+h{\hat{s}}_{x,p},{\hat{d}}_G,{\bar{w}}_k)+f({\hat{x}}_k-h{\hat{s}}_{x,p},{\hat{d}}_G,{\bar{w}}_k))---\left(20\right)$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_v}{\Sigma }}(f({\hat{x}}_k,{\hat{d}}_G,{\bar{w}}_k+h{\hat{s}}_{w,p})+f({\hat{x}}_{k,}{\hat{d}}_G,{\bar{w}}_k-h{\hat{s}}_{w,p}))$

其中，表示w(k)的均值，n_x表示状态向量的维数，n_w表示系统噪声的维数，要保证估计结果是无偏估计插值步长取h²＝3。

状态预测误差方差阵的Cholesky分解是经Householder变换后的矩阵：

$\bar{S}(k+1)=\left[S_{x\hat{x}}^{\left(1\right)}\left(k\right)S_{\mathrm{xw}}^{\left(1\right)}\left(k\right)S_{x\hat{x}}^{\left(2\right)}\left(k\right)S_{\mathrm{xw}}^{\left(2\right)}\left(k\right)\right]---\left(21\right)$

其中，

$S_{x\hat{x}}^{\left(1\right)}\left(k\right)=\left\{S_{x\hat{x}}^{\left(1\right)}{\left(k\right)}_{(i,j)}\right\}=\{(f_i({\hat{x}}_k+h{\hat{s}}_{x,j},{\hat{d}}_G,{\bar{w}}_k)-f_i({\hat{x}}_k-h{\hat{s}}_{x,j},{\hat{d}}_G,{\bar{w}}_k))/2h\},$

$S_{xw}^{\left(1\right)}\left(k\right)=\left\{S_{xw}^{\left(1\right)}{\left(k\right)}_{(i,j)}\right\}=\{(f_i({\hat{x}}_{k,}{\hat{d}}_G,{\bar{w}}_k+h{\hat{s}}_{w,j})-f_i({\hat{x}}_k,{\hat{d}}_G,{\bar{w}}_k-h{\hat{s}}_{w,j}))/2h\},$

$S_{x\hat{x}}^{\left(2\right)}\left(k\right)=\left\{S_{x\hat{x}}^{\left(2\right)}{\left(k\right)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(f_i({\hat{x}}_k+h{\hat{s}}_{x,j},{\hat{d}}_G,{\bar{w}}_k)$

$+f_i({\hat{x}}_k-h{\hat{s}}_{x,j},{\hat{d}}_G,{\bar{w}}_k)-2f_i({\hat{x}}_k,{\hat{d}}_G,{\bar{w}}_k)\left)\right\},$

$S_{xw}^{\left(2\right)}\left(k\right)=\left\{S_{xw}^{\left(2\right)}{\left(k\right)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(f_i({\hat{x}}_{k,}{\hat{d}}_G,{\bar{w}}_k+h{\hat{s}}_{w,j})$

$+f_i({\hat{x}}_{k,}{\hat{d}}_G,{\bar{w}}_k-h{\hat{s}}_{w,j})-2f_i({\hat{x}}_k,{\hat{d}}_G,{\bar{w}}_k)\left)\right\},$

其中，表示的第j列，表示的第j列，表示S_w的第j列。观测值的一步预测为：

${\bar{y}}_{k+1}=\frac{h^2-n_x-n_v}{h^2}g({\bar{x}}_{k+1},{\bar{v}}_{k+1})$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_x}{\Sigma }}(g({\bar{x}}_{k+1}+h{\bar{s}}_{x,p},{\bar{v}}_{k+1})+g({\bar{x}}_{k+1}-h{\bar{s}}_{x,p},{\bar{v}}_{k+1}))---\left(22\right)$

$+\frac{1}{2h^2}\underset{p=1}{\overset{n_v}{\Sigma }}(g({\bar{x}}_{k+1,}{\bar{v}}_{k+1}+h{\bar{s}}_{v,p})+g({\bar{x}}_{k+1,}{\bar{v}}_{k+1}-h{\bar{s}}_{v,p}))$

其中，n_v是测量噪声维数。的误差方差阵的Cholesky分解是S_y(k+1)经Householder变换后的矩阵：

$S_y(k+1)=\left[S_{y\bar{x}}^{\left(1\right)}(k+1)S_{\mathrm{yv}}^{\left(1\right)}(k+1)S_{y\bar{x}}^{\left(2\right)}(k+1)S_{\mathrm{yv}}^{\left(2\right)}(k+1)\right]---\left(23\right)$

其中，

$S_{y\bar{x}}^{\left(1\right)}(k+1)=\left\{S_{y\bar{x}}^{\left(1\right)}{(k+1)}_{(i,j)}\right\}=\{(g_i({\bar{x}}_{k+1}+h{\bar{s}}_{x,j},{\bar{v}}_{k+1})-g_i({\bar{x}}_{k+1}-h{\bar{s}}_{x,j},{\bar{v}}_{k+1}))/2h\},$

$S_{\mathrm{yv}}^{\left(1\right)}(k+1)=\left\{S_{\mathrm{yv}}^{\left(1\right)}{(k+1)}_{(i,j)}\right\}=\{(g_i({\bar{x}}_{k+1,}{\bar{v}}_{k+1}+h{\bar{s}}_{v,j})-g_i({\bar{x}}_{k+1,}{\bar{v}}_{k+1}-h{\bar{s}}_{v,j}))/2h\},$

$S_{y\bar{x}}^{\left(2\right)}(k+1)=\left\{S_{y\bar{x}}^{\left(2\right)}{(k+1)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(g_i({\bar{x}}_{k+1}+h{\bar{s}}_{x,j},{\bar{v}}_{k+1})$

$+g_i({\bar{x}}_{k+1}-h{\bar{s}}_{x,j},{\bar{v}}_{k+1})-2g_i({\bar{x}}_{k+1},{\bar{v}}_{k+1})\left)\right\},$

$S_{\mathrm{yv}}^{\left(2\right)}(k+1)=\left\{S_{\mathrm{yv}}^{\left(2\right)}{(k+1)}_{(i,j)}\right\}=\{\frac{\sqrt{h^2-1}}{2h^2}(g_i({\bar{x}}_{k+1},{\bar{v}}_{k+1}+h{\bar{s}}_{v,j})$

$+g_i({\bar{x}}_{k+1},{\bar{v}}_{k+1}-h{\stackrel{\text{-}}{s}}_{v,j})-2g_i({\bar{x}}_{k+1},{\bar{v}}_{k+1})\left)\right\},$

式中：表示S_v的第j列。

状态和测量的互协方差阵为：

$P_{\mathrm{xy}}(k+1)={\bar{S}}_x(k+1)S_{y\bar{x}}{(k+1)}^T---\left(24\right)$

由式(24)得到增益矩阵K_k+1为：

K_k+1＝P_xy(k+1)[S_y(k+1)S_y(k+1)^T]^-1    (25)

基于测量的状态及估计误差方差阵更新为：

${\hat{x}}_{k+1}={\stackrel{-}{x}}_{k+1}+K_{k+1}(y_{k+1}-{\bar{y}}_{k+1})---\left(26\right)$

$\hat{P}(k+1)={[{\bar{S}}_x(k+1)-K_{k+1}{S}_{y\bar{x}}^{\left(1\right)}(k+1)][{\bar{S}}_x(k+1)-K_{k+1}{S}_{\mathrm{yx}}^{\left(1\right)}(k+1)]}^T+K_{k+1}{S}_{\mathrm{yv}}^{\left(1\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yv}}^{\left(1\right)}(k+1)\right]}^T$

$+K_{k+1}{S}_{\mathrm{yx}}^{\left(2\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yx}}^{\left(2\right)}(k+1)\right]}^T+K_{k+1}{S}_{\mathrm{yv}}^{\left(2\right)}(k+1){\left[K_{k+1}{S}_{\mathrm{yv}}^{\left(2\right)}(k+1)\right]}^T---\left(27\right)$

由此得到状态估计值其中即为卫星的姿态四元数，每次估计之后对姿态四元数进行归一化处理，并且按照式(28)(29)(30)解算得到姿态角；

俯仰角：θ＝-arcsin(2_q1q3-2_q2q4)    (28)

偏航角：$\Psi =\arctan \left(\frac{2_{q2q3}-2_{q1q4}}{q_3^2+q_4^2-q_1^2-q_2^2}\right)---\left(29\right)$

滚转角：

本方法因为有对模型误差实时估计的能力，可以实时地补偿模型误差，因此可以得到更高精度的卫星姿态估计。

同时，本方法还可以适用于其他类似带有模型误差的系统。

当存在模型误差时，在同样的仿真条件下，应用本发明确定卫星的姿态与应用扩展卡尔曼滤波(EKF)仿真结果的误差曲线如图2a、图2b、图2c所示，应用本发明确定卫星的姿态与二阶插值滤波(DD2)仿真结果的误差曲线如图3a、图3b、图3c所示。

图2a和图3a中横坐标均是仿真时间，纵坐标是俯仰角的估计误差曲线(真实值与估计值之差)，从图中可以看出，EKF和DD2的俯仰角估计误差在10角秒以内，而本发明的估计误差在1角秒以内。

图2b和图3b中横坐标均是仿真时间，纵坐标是偏航角的估计误差曲线；图2c和图3c的横坐标均是仿真时间，纵坐标是滚转角的估计误差曲线；从图中可以看出，EKF和DD2的偏航角和滚转角估计误差震荡比较剧烈，甚至有时候误差已经超过15角秒，而本发明的估计误差因为能够实时估计模型误差并补偿，所以估计误差角仍然在2角秒以内；因此本发明提出的方法精度有明显提高。