一种基于星敏感器和陀螺的高精度卫星姿态确定方法-山东亚星游戏官网机床有限公司

专利名称：：一种基于星敏感器和陀螺的高精度卫星姿态确定方法
技术领域：
：本发明涉及一种基于星敏感器和陀螺的高精度卫星姿态确定方法，属于空间飞行器高精度姿态确定
技术领域：
。
背景技术：
：飞行器姿态确定系统是飞行器姿态控制系统中的重要组成部分，其精度和可靠性是飞行器长期正常运行的重要保证和前提。该系统主要由姿态敏感器和相应姿态确定算法组成，为了克服参考矢量的不确定性对姿态确定精度的影响，状态估计的方法在高精度姿态确定场合得到了广泛的应用。目前常用的方法是扩展卡尔曼滤波(ExtendedKalmanFilter,EKF)，EKF具有收敛速度快，算法简单，可以实时解算等优点。但是EKF因为采用了对非线性模型Taylor展开近似的方法，在非线性较强时估计偏差较大。并且EKF需要计算雅克比矩阵，要求函数必须可微，限制了其使用范围。此外，EKF将系统噪声处理为零均值白噪声，这就要求建模必须十分精确，若实际系统中的噪声不是高斯白噪声，而被作为白噪声来处理，则将导致估计精度严重下降。非线性预测滤波(NonlinearPredictFilter,NPF)考虑了系统模型误差或未知输入的影响，具有对模型误差的估计能力，与EKF等其他非线性滤波方法相比，预测滤波能够在线估计出未知模型误差，克服了将模型误差和系统噪声设为高斯白噪声的局限，在飞行器姿态估计领域中得到了应用。但是预测滤波收敛速度相对较慢，收敛时估计出的模型误差的不准确会对滤波收敛性和精度造成影响。
发明内容本发明的目的是为了解决飞行器高精度姿态确定方面现有技术的不足，提出一种基于星敏感器和陀螺的高精度卫星姿态确定方法。本发明的一种基于星敏感器和陀螺的高精度卫星姿态确定方法，包括以下几个步骤步骤一建立卫星姿态确定系统的状态方程；步骤二建立卫星姿态确定系统的测量方程；步骤三利用预测滤波在线实时估计模型误差；步骤四对补偿后的模型利用二阶插值滤波进行状态估计，得到卫星的姿态。本发明的优点在于(1)采用了预测滤波在线实时估计模型误差并修正系统模型，克服了传统估计过程中将误差处理为零均值白噪声的缺点。(2)采用了二阶插值滤波算法，是一种比EKF精度更高的多项式插值滤波算法，且不需要计算偏导数，适用范围更广；(3)本发明采用了预测滤波估计并补偿系统模型误差，可以处理任何非线性系统和噪声情况，获得更高精度的估计结果，适用于高精度姿态确定领域。图1是本发明的方法流程图；图2a是本发明的方法与EKF方法仿真结果的俯仰角误差曲线对比图；图2b是本发明的方法与EKF方法仿真结果的偏航角误差曲线对比图；图2c是本发明的方法与EKF方法仿真结果的滚转角误差曲线对比图；图3a是本发明的方法与二阶插值滤波方法仿真结果的俯仰角误差曲线对比图；图3b是本发明的方法与二阶插值滤波方法仿真结果的偏航角误差曲线对比图；图3c是本发明的方法与二阶插值滤波方法仿真结果的滚转角误差曲线对比图。具体实施例方式下面将结合附图和实施例对本发明作进一步的详细说明。本发明是一种基于星光和陀螺的高精度卫星姿态确定方法，利用陀螺和星敏感器作为姿态敏感器，利用预测滤波的思想实时估计出卫星姿态确定系统的模型误差并实时进行补偿，再利用精度更高的二阶插值滤波估计出卫星本体坐标系相对于惯性坐标系的姿态四元数，然后经过解算得到卫星姿态，流程如图1所示，包括以下几个步骤步骤一建立卫星姿态确定系统的状态方程；用姿态动力学方程与运动学方程一起来描述卫星的姿态变化，姿态动力学方程可从刚体的动量矩公式和定理导出，即欧拉_牛顿法根据刚体动量矩定理，卫星的姿态动力学方程为(1)式中，ω=[ωχ，coy，ωJt为卫星本体坐标系对惯性坐标系的转动角速度，ωχ、ωy、ωζ为卫星本体坐标系对惯性坐标系在卫星本体坐标系下沿x，y，z三个轴的分量；J为卫星的惯性张量矩阵；Gmb为地磁力矩；Gab为气动力矩；Gsb为太阳光压力矩；Ggb为重力梯度力矩；Ggb具体为(2)式中，μ为地心引力常数；r为卫星的地心距；Rb为卫星在卫星本体坐标系下的位置矢量，具体为(3)式中，Ri为卫星在惯性坐标系下的位置矢量，Cf为从惯性坐标系到卫星本体坐标系的转换矩阵，具体为地心距真近点角近点角；平近点角M=n(t-tO)；η为卫星的828平运动速度；、为卫星到达近地点的时间；t为时间；C〗为地心轨道坐标系到惯性坐标系的转换矩阵，具体为其中，Ω。为卫星轨道升交点赤经；i。为卫星轨道倾角；W。为卫星轨道近地点幅角。因为Ggb比地磁力矩Gmb、气动力矩Gab和太阳光压力矩Gsb对卫星姿态确定系统的影响大得多，所以将Gmb、Gab和Gsb用合力矩用de表示，作为未建模的模型误差，则式(1)的卫星姿态动力学方程写为姿态运动学方程是姿态参数在姿态机动过程中变化的方程，由于四元数表示的姿态运动学方程为线性微分方程，不含三角函数，无奇点问题，所以采用四元数来描述的卫星姿态运动学方程如下(5)式中，q=[如仏,仏，仏]7^=^3，^Γ为卫星本体坐标系相对于惯性坐标系的姿态四元数；为单位阵；]分别表示由向量co、q13的分量构成的反对称矩阵，[ωχ]=ωζ0-ωχ；由式(4)和式(5)构成卫星姿态确定系统的状态方程为式中x为状态向量，χ=[ωτ，qT]T；G为误差扰动矩阵，G=；系统噪声w(k)(0，Q(k))，即w(k)服从均值为0、方差为Q(k)的高斯分布。将式(6)简记为(7)步骤二建立卫星姿态确定系统的测量方程；本发明中采用三轴陀螺和星敏感器作为姿态敏感器。1)陀螺；陀螺的测量方程为式中，gl为陀螺的测量输出，记卫星本体坐标系对惯性坐标系的转动角速度，Vi(i=χ,y，ζ)为陀螺测量的高斯白噪声。(2)星敏感器；本发明采用两个星敏感器进行测量，在惯性坐标系中两个星敏感的主光轴的单位方向矢量分别是Iil和Ii2，则星敏感器的测量方程为(9)式中，vsl、vs2是星敏感器测量的噪声，这里主要考虑为高斯白噪声；gl、&分别是两个星敏感器的测量输出；记；Cf是由惯性坐标系到卫星本体坐标系的转换矩阵，可用姿态四元数表示，具体为(3)建立卫星姿态确定系统的观测方程为将式(11)简记为其中，即v(k)服从均值为0，方差为R(k)的高斯分布，并且w(k)，v(k)相互独立。步骤三利用预测滤波在线实时估计模型误差；模型误差ac(A)的估计式为式中j)(幻为陀螺和两个星敏感器的输出估计，T为滤波周期，U为灵敏度矩阵，A(T)为对角矩阵，为一个列向量。W为加权矩阵，预先设定数值。其中，外幻具体为(14)式中i(A;)为k时刻的状态量的估计值，;^t)=0,表示ν(k)的均值。灵敏度矩阵U为相关的李导数的具体为对角矩阵Λ(T)为其中列向量Z[i(A:),:r]为上式中各阶李导数的为最后，根据式(13)得到模型误差的估计值知(A:)。步骤四对补偿后的模型利用二阶插值滤波进行状态估计，得到卫星的姿态；插值滤波是利用Stirling插值公式将非线性函数进行近似，是一种基于最小均方误差准则的估计方法。具体为将步骤三中得到的模型误差知(幻代入状态方程(7)进行补偿，将补偿后的卫星姿态确定系统模型的状态方程和测量方程写成离散的非线性形式，为(18)yk=g(xk,vk)设滤波值^的误差方差阵§的平方根为I，即&是§的Cholesky分解，模型误差方差阵Q的平方根为Sw，测量噪声的误差方差阵R的平方根为Sv，状态预测误差的方差阵ρ的平方根为I,即状态及其误差方差阵的一步预测_其中，5(0=0,表示W(k)的均值，~表示状态向量的维数，nw表示系统噪声的维数，要保证估计结果是无偏估计插值步长取h2=3。状态预测误差方差阵的Cholesky分解是+经Householder变换后的矩阵其中，其中，表示t的第j列，^表示良的第j列，L，y表示Sw的第j列。观测值的一步预测为其中，nv是测量噪声维数。yk+1的误差方差阵的Cholesky分解是Sy(k+1)经Householder变换后的矩阵(23)其中，式中sv,j表示Sv的第j列。状态和测量的互协方差阵为由式(24)得到增益矩阵Kk+1为基于测量的状态及估计误差方差阵更新为由此得到状态估计值其中q即为卫星的姿态四元数，每次估计之后对姿态四元数进行归一化处理，并且按照式(28)(29)(30)解算得到姿态角；俯仰角θ=-arcsin(2qlq3-2q2q1)(28)偏航角滚转角本方法因为有对模型误差实时估计的能力，可以实时地补偿模型误差，因此可以得到更高精度的卫星姿态估计。同时，本方法还可以适用于其他类似带有模型误差的系统。当存在模型误差时，在同样的仿真条件下，应用本发明确定卫星的姿态与应用扩展卡尔曼滤波(EKF)仿真结果的误差曲线如图2a、图2b、图2c所示，应用本发明确定卫星的姿态与二阶插值滤波(DD2)仿真结果的误差曲线如图3a、图3b、图3c所示。图2a和图3a中横坐标均是仿真时间，纵坐标是俯仰角的估计误差曲线(真实值与估计值之差)，从图中可以看出，EKF和DD2的俯仰角估计误差在10角秒以内，而本发明的估计误差在1角秒以内。图2b和图3b中横坐标均是仿真时间，纵坐标是偏航角的估计误差曲线；图2c和图3c的横坐标均是仿真时间，纵坐标是滚转角的估计误差曲线；从图中可以看出，EKF和DD2的偏航角和滚转角估计误差震荡比较剧烈，甚至有时候误差已经超过15角秒，而本发明的估计误差因为能够实时估计模型误差并补偿，所以估计误差角仍然在2角秒以内；因此本发明提出的方法精度有明显提高。权利要求一种基于星光和陀螺的高精度卫星姿态确定方法，其特征在于，采用陀螺和星敏感器作为姿态敏感器，利用预测滤波算法实时估计出卫星姿态确定系统的模型误差并实时进行补偿，再利用二阶插值滤波进行状态估计，具体包括以下几个步骤步骤一建立卫星姿态确定系统的状态方程；卫星的姿态动力学方程为<mrow><mi>J</mi><mover><mi>ω</mi><mo>·</mo></mover><mo>+</mo><mi>ω</mi><mo>×</mo><mi>Jω</mi><mo>=</mo><msub><mi>G</mi><mi>gb</mi></msub><mo>+</mo><msub><mi>G</mi><mi>gb</mi></msub><mo>+</mo><msub><mi>G</mi><mi>gb</mi></msub><mo>+</mo><msub><mi>G</mi><mi>sb</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>式中，ω＝[ωx，ωy，ωz]T为卫星本体坐标系对惯性坐标系的转动角速度，ωx、ωy、ωz为卫星本体坐标系对惯性坐标系在卫星本体坐标系下沿x，y，z三个轴的分量；J为卫星的惯性张量矩阵；Gmb为地磁力矩；Gab为气动力矩；Gsb为太阳光压力矩；Ggb为重力梯度力矩；Ggb具体为<mrow><msub><mi>G</mi><mi>gb</mi></msub><mo>=</mo><mfrac><mrow><mn>3</mn><mi>μ</mi></mrow><mrow><msup><mi>r</mi><mn>5</mn></msup><mi></mi></mrow></mfrac><msub><mi>R</mi><mi>b</mi></msub><mo>×</mo><mi>J</mi><msub><mi>R</mi><mi>b</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></mrow>式中，μ为地心引力常数；r为卫星的地心距；Rb为卫星在卫星本体坐标系下的位置矢量，具体为<mrow><msub><mi>R</mi><mi>b</mi></msub><mo>=</mo><msubsup><mi>C</mi><mi>i</mi><mi>b</mi></msubsup><msub><mi>R</mi><mi>i</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>式中，Ri为卫星在惯性坐标系下的位置矢量，为从惯性坐标系到卫星本体坐标系的转换矩阵，具体为地心距真近点角偏近点角平近点角M＝n(t-t0)；n为卫星的平运动速度；t0为卫星到达近地点的时间；t为时间；为地心轨道坐标系到惯性坐标系的转换矩阵，具体为<mrow><msubsup><mi>C</mi><mi>o</mi><mi>i</mi></msubsup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mi>cos</mi><msub><mi>Ω</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>w</mi><mi>o</mi></msub><mo>-</mo><mi>sin</mi><msub><mi>w</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>i</mi><mi>o</mi></msub><mi>sin</mi><msub><mi>Ω</mi><mi>o</mi></msub></mtd><mtd><mo>-</mo><mi>cos</mi><msub><mi>Ω</mi><mi>o</mi></msub><mi>sin</mi><msub><mi>w</mi><mi>o</mi></msub><mo>-</mo><mi>cos</mi><msub><mi>w</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>i</mi><mi>o</mi></msub><mi>sin</mi><msub><mi>Ω</mi><mi>o</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>Ω</mi><mi>o</mi></msub><mi>sin</mi><msub><mi>i</mi><mi>o</mi></msub></mtd></mtr><mtr><mtd><mi>sin</mi><msub><mi>Ω</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>w</mi><mi>o</mi></msub><mo>+</mo><mi>sin</mi><msub><mi>w</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>i</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>Ω</mi><mi>o</mi></msub></mtd><mtd><mo>-</mo><mi>sin</mi><msub><mi>Ω</mi><mi>o</mi></msub><mi>sin</mi><msub><mi>w</mi><mi>o</mi></msub><mo>+</mo><mi>cos</mi><msub><mi>w</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>i</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>Ω</mi><mi>o</mi></msub></mtd><mtd><mo>-</mo><mi>cos</mi><msub><mi>Ω</mi><mi>o</mi></msub><mi>sin</mi><msub><mi>i</mi><mi>o</mi></msub></mtd></mtr><mtr><mtd><mi>sin</mi><msub><mi>w</mi><mi>o</mi></msub><mi>sin</mi><msub><mi>i</mi><mi>o</mi></msub></mtd><mtd><mi>sin</mi><msub><mi>w</mi><mi>o</mi></msub><mi>cos</mi><msub><mi>i</mi><mi>o</mi></msub></mtd><mtd><mi>cos</mi><msub><mi>i</mi><mi>o</mi></msub></mtd></mtr></mtable></mfenced></mrow>其中，Ωo为卫星轨道升交点赤经；io为卫星轨道倾角；wo为卫星轨道近地点幅角；将Gmb、Gab和Gsb的合力矩表示为dG，作为未建模的模型误差，则式(1)的卫星姿态动力学方程写为<mrow><mover><mi>ω</mi><mo>·</mo></mover><mo>=</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mrow><mo>(</mo><mo>-</mo><mi>ω</mi><mo>×</mo><mi>Jω</mi><mo>+</mo><msub><mi>G</mi><mi>gb</mi></msub><mo>)</mo></mrow><mo>+</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><msub><mi>d</mi><mi>G</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow>采用四元数来描述的卫星姿态运动学方程如下<mrow><mover><mi>q</mi><mo>·</mo></mover><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mi>Ω</mi><mrow><mo>(</mo><mi>ω</mi><mo>)</mo></mrow><mi>q</mi><mo>=</mo><mfrac><mn>1</mn><mn>2</mn></mfrac><mi>Ξ</mi><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>ω</mi><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>5</mn><mo>)</mo></mrow></mrow>式中，为卫星本体坐标系相对于惯性坐标系的姿态四元数；q13＝[q1q2q3]T，I3×3为单位阵；[ω×]、[q13×]分别表示由向量ω、q13的分量构成的反对称矩阵，由式(4)和式(5)构成卫星姿态确定系统的状态方程为<mrow><mover><mi>x</mi><mo>·</mo></mover><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mrow><mo>(</mo><mo>-</mo><mi>ω</mi><mo>×</mo><mi>Jω</mi><mo>+</mo><msub><mi>G</mi><mi>gb</mi></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mfrac><mn>1</mn><mn>2</mn></mfrac><mi>Ξ</mi><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>ω</mi></mtd></mtr></mtable></mfenced><mo>+</mo><mi>G</mi><mo>·</mo><msub><mi>d</mi><mi>G</mi></msub><mo>+</mo><mi>W</mi><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>6</mn><mo>)</mo></mrow></mrow>式中x为状态向量，x＝[ωT，qT]T；G为误差扰动矩阵，系统噪声w(k)～(0，Q(k))，即w(k)服从均值为0、方差为Q(k)的高斯分布；将式(6)简记为<mrow><mover><mi>x</mi><mo>·</mo></mover><mo>=</mo><mi>f</mi><mrow><mo>(</mo><mi>x</mi><mo>,</mo><msub><mi>d</mi><mi>G</mi></msub><mo>,</mo><mi>w</mi><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>7</mn><mo>)</mo></mrow></mrow>步骤二建立卫星姿态确定系统的测量方程；采用三轴陀螺和两个星敏感器作为姿态敏感器；1)陀螺；陀螺的测量方程为<mrow><msub><mi>g</mi><mn>1</mn></msub><mrow><mo>(</mo><mi>x</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>ω</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ω</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>ω</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mo>+</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>v</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>v</mi><mi>y</mi></msub></mtd></mtr><mtr><mtd><msub><mi>v</mi><mi>z</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><msub><mi>g</mi><mi>ω</mi></msub><mrow><mo>(</mo><mi>x</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>)</mo></mrow><mo>+</mo><msub><mi>v</mi><mi>ω</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>8</mn><mo>)</mo></mrow></mrow>式中，g1为陀螺的测量输出，记ωi为卫星本体坐标系对惯性坐标系的转动角速度，vi为陀螺测量的高斯白噪声，i＝x，y，z；2)星敏感器；在惯性坐标系中两个星敏感的主光轴的单位方向矢量分别是li1和li2，则星敏感器的测量方程为<mfencedopen='{'close=''separators=''><mtable><mtr><mtd><msub><mi>g</mi><mn>2</mn></msub><mo>=</mo><msubsup><mi>C</mi><mi>i</mi><mi>b</mi></msubsup><msub><mi>I</mi><mrow><mi>i</mi><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>v</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>=</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mrow><mo>(</mo><mi>x</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>)</mo></mrow><mo>+</mo><msub><mi>v</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mi>g</mi><mn>3</mn></msub><mo>=</mo><msubsup><mi>C</mi><mi>i</mi><mi>b</mi></msubsup><msub><mi>I</mi><mrow><mi>i</mi><mn>2</mn></mrow></msub><mo>+</mo><msub><mi>v</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>=</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mrow><mo>(</mo><mi>x</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>)</mo></mrow><mo>+</mo><msub><mi>v</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub></mtd></mtr></mtable><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>9</mn><mo>)</mo></mrow></mfenced>式中，vs1、vs2是星敏感器测量的高斯白噪声；g1、g2分别是两个星敏感器的测量输出；记是由惯性坐标系到卫星本体坐标系的转换矩阵，用姿态四元数表示，具体为<mrow><msubsup><mi>C</mi><mi>i</mi><mi>b</mi></msubsup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>4</mn><mn>2</mn></msubsup></mtd><mtd><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>+</mo><msub><mi>q</mi><mn>3</mn></msub><msub><mi>q</mi><mn>4</mn></msub><mo>)</mo></mrow></mtd><mtd><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>-</mo><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>4</mn></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>2</mn></msub><mo>-</mo><msub><mi>q</mi><mn>3</mn></msub><msub><mi>q</mi><mn>4</mn></msub><mo>)</mo></mrow></mtd><mtd><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>4</mn><mn>2</mn></msubsup></mtd><mtd><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>+</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>4</mn></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>+</mo><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>4</mn></msub><mo>)</mo></mrow></mtd><mtd><mn>2</mn><mrow><mo>(</mo><msub><mi>q</mi><mn>2</mn></msub><msub><mi>q</mi><mn>3</mn></msub><mo>-</mo><msub><mi>q</mi><mn>1</mn></msub><msub><mi>q</mi><mn>4</mn></msub><mo>)</mo></mrow></mtd><mtd><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>4</mn><mn>2</mn></msubsup></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow>3)建立卫星姿态确定系统的观测方程为<mrow><mi>y</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>g</mi><mi>ω</mi></msub><mrow><mo>(</mo><mi>x</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mrow><mo>(</mo><mi>x</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mrow><mo>(</mo><mi>x</mi><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>+</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>v</mi><mi>ω</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>v</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>v</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>11</mn><mo>)</mo></mrow></mrow>将式(11)简记为y(k)＝g(x(k)，v(k))(12)其中，即v(k)服从均值为0、方差为R(k)的高斯分布，并且w(k)，v(k)相互独立；步骤三利用预测滤波在线实时估计模型误差；模型误差的估计式为<mrow><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mo>-</mo><msup><mrow><mo>(</mo><msup><mrow><mo>{</mo><mi>Λ</mi><mrow><mo>(</mo><mi>T</mi><mo>)</mo></mrow><mi>U</mi><mo>[</mo><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>]</mo><mo>}</mo></mrow><mi>T</mi></msup><msup><mi>R</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>{</mo><mi>Λ</mi><mrow><mo>(</mo><mi>T</mi><mo>)</mo></mrow><mi>U</mi><mo>[</mo><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>]</mo><mo>}</mo><mo>+</mo><mi>W</mi><mo>)</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup><msup><mrow><mo>{</mo><mi>Λ</mi><mrow><mo>(</mo><mi>T</mi><mo>)</mo></mrow><mi>U</mi><mo>[</mo><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>]</mo><mo>}</mo></mrow><mi>T</mi></msup><msup><mi>R</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup></mrow>(13)<mrow><mo>×</mo><mo>{</mo><mover><mi>y</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>+</mo><mi>z</mi><mo>[</mo><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>,</mo><mi>T</mi><mo>]</mo><mo>-</mo><mi>y</mi><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>}</mo></mrow>式中为陀螺和两个星敏感器的输出估计，T为滤波周期，U为灵敏度矩阵，Λ(T)为对角矩阵，为一个列向量；W为加权矩阵，预先设定数值；其中，具体为<mrow><mover><mi>y</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mi>g</mi><mo>[</mo><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>,</mo><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>]</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>14</mn><mo>)</mo></mrow></mrow>式中为k时刻的状态量的估计值；表示v(k)的均值；灵敏度矩阵U为<mrow><mi>U</mi><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>L</mi><mi>G</mi></msub><mo>[</mo><msub><msup><mi>L</mi><mn>0</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mi>w</mi></msub><mo>)</mo></mrow><mo>]</mo></mtd></mtr><mtr><mtd><msub><mi>L</mi><mi>G</mi></msub><mo>[</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>]</mo></mtd></mtr><mtr><mtd><msub><mi>L</mi><mi>G</mi></msub><mo>[</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mo>]</mo></mtd></mtr></mtable></mfenced><mo>,</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>15</mn><mo>)</mo></mrow></mrow>相关的李导数的具体为<mrow><msub><mi>L</mi><mi>G</mi></msub><mo>[</mo><msub><msup><mi>L</mi><mn>0</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mi>w</mi></msub><mo>)</mo></mrow><mo>]</mo><mo>=</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup></mrow><mrow><msub><mi>L</mi><mi>G</mi></msub><mo>[</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>]</mo><mo>=</mo><mfrac><mrow><mo>&PartialD;</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow></mrow><mrow><mo>&PartialD;</mo><mover><mi>x</mi><mo>^</mo></mover></mrow></mfrac><mo>·</mo><mi>G</mi><mo>=</mo><mo>-</mo><msup><mi>Ξ</mi><mi>T</mi></msup><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mi>Γ</mi><mrow><mo>(</mo><msub><mi>l</mi><mrow><mi>i</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mi>Ξ</mi><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup></mrow><mrow><msub><mi>L</mi><mi>G</mi></msub><mo>[</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mo>]</mo><mo>=</mo><mfrac><mrow><mo>&PartialD;</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow></mrow><mrow><mo>&PartialD;</mo><mover><mi>x</mi><mo>^</mo></mover></mrow></mfrac><mo>·</mo><mi>G</mi><mo>=</mo><mo>-</mo><msup><mi>Ξ</mi><mi>T</mi></msup><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mi>Γ</mi><mrow><mo>(</mo><msub><mi>l</mi><mrow><mi>i</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mi>Ξ</mi><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup></mrow>对角矩阵Λ(T)为<mrow><mi>Λ</mi><mrow><mo>(</mo><mi>T</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>Λ</mi><mi>w</mi></msub></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><msub><mi>Λ</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mrow><msub><mi>Λ</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>16</mn><mo>)</mo></mrow></mrow>其中Λw＝T·I3×3，<mrow><msub><mi>Λ</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>=</mo><mfrac><msup><mi>T</mi><mn>2</mn></msup><mn>2</mn></mfrac><mo>·</mo><msub><mi>I</mi><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub><mo>,</mo></mrow><mrow><msub><mi>Λ</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>=</mo><mfrac><msup><mi>T</mi><mn>2</mn></msup><mn>2</mn></mfrac><mo>·</mo><msub><mi>I</mi><mrow><mn>3</mn><mo>×</mo><mn>3</mn></mrow></msub><mo>,</mo><mo>;</mo></mrow>列向量为<mrow><mi>z</mi><mo>[</mo><mover><mi>x</mi><mo>^</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>,</mo><mi>T</mi><mo>]</mo><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>z</mi><mi>ω</mi></msub></mtd></mtr><mtr><mtd><msub><mi>z</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mi>z</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mi>T</mi><mo>·</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mi>ω</mi></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mi>T</mi><mo>·</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>+</mo><mfrac><msup><mi>T</mi><mn>2</mn></msup><mn>2</mn></mfrac><mo>·</mo><msub><msup><mi>L</mi><mn>2</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mi>T</mi><mo>·</mo><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mo>+</mo><mfrac><msup><mi>T</mi><mn>2</mn></msup><mn>2</mn></mfrac><mo>·</mo><msub><msup><mi>L</mi><mn>2</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>17</mn><mo>)</mo></mrow></mrow>上式中各阶李导数的为<mrow><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mi>ω</mi></msub><mo>)</mo></mrow><mo>=</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>[</mo><mo>-</mo><mover><mi>ω</mi><mo>^</mo></mover><mo>×</mo><mi>J</mi><mover><mi>ω</mi><mo>^</mo></mover><mo>+</mo><msub><mi>G</mi><mi>gb</mi></msub><mo>]</mo></mrow><mrow><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>=</mo><mo>-</mo><msup><mi>Ξ</mi><mi>T</mi></msup><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mi>Γ</mi><mrow><mo>(</mo><msub><mi>l</mi><mrow><mi>i</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mi>Ξ</mi><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mover><mi>ω</mi><mo>^</mo></mover></mrow><mrow><msub><msup><mi>L</mi><mn>2</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>=</mo><mo>[</mo><mover><mi>ω</mi><mo>^</mo></mover><mo>×</mo><mo>]</mo><mo>·</mo><msup><mi>Ξ</mi><mi>T</mi></msup><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mi>Γ</mi><mrow><mo>(</mo><msub><mi>l</mi><mrow><mi>i</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mi>Ξ</mi><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mover><mi>ω</mi><mo>^</mo></mover><mo>-</mo><msup><mi>Ξ</mi><mi>T</mi></msup><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mi>Γ</mi><mrow><mo>(</mo><msub><mi>l</mi><mrow><mi>i</mi><mn>1</mn></mrow></msub><mo>)</mo></mrow><mi>Ξ</mi><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mo>×</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>[</mo><mo>-</mo><mover><mi>ω</mi><mo>^</mo></mover><mo>×</mo><mi>J</mi><mover><mi>ω</mi><mo>^</mo></mover><mo>+</mo><msub><mi>G</mi><mi>gb</mi></msub><mo>]</mo></mrow><mrow><msub><msup><mi>L</mi><mn>1</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mo>=</mo><mo>-</mo><msup><mi>Ξ</mi><mi>T</mi></msup><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mi>Γ</mi><mrow><mo>(</mo><msub><mi>l</mi><mrow><mi>i</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mi>Ξ</mi><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mover><mi>ω</mi><mo>^</mo></mover></mrow><mrow><msub><msup><mi>L</mi><mn>2</mn></msup><mi>f</mi></msub><mrow><mo>(</mo><msub><mi>g</mi><mrow><mi>s</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mo>=</mo><mo>[</mo><mover><mi>ω</mi><mo>^</mo></mover><mo>×</mo><mo>]</mo><mo>·</mo><msup><mi>Ξ</mi><mi>T</mi></msup><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mi>Γ</mi><mrow><mo>(</mo><msub><mi>l</mi><mrow><mi>i</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mi>Ξ</mi><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mover><mi>ω</mi><mo>^</mo></mover><mo>-</mo><msup><mi>Ξ</mi><mi>T</mi></msup><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mi>Γ</mi><mrow><mo>(</mo><msub><mi>l</mi><mrow><mi>i</mi><mn>2</mn></mrow></msub><mo>)</mo></mrow><mi>Ξ</mi><mrow><mo>(</mo><mover><mi>q</mi><mo>^</mo></mover><mo>)</mo></mrow><mo>×</mo><msup><mi>J</mi><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>[</mo><mo>-</mo><mover><mi>ω</mi><mo>^</mo></mover><mo>×</mo><mi>J</mi><mover><mi>ω</mi><mo>^</mo></mover><mo>+</mo><msub><mi>G</mi><mi>gb</mi></msub><mo>]</mo></mrow>其中<mrow><mi>Γ</mi><mrow><mo>(</mo><mi>a</mi><mo>)</mo></mrow><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mo>-</mo><mo>[</mo><mi>a</mi><mo>×</mo><mo>]</mo></mtd><mtd><mo>-</mo><mi>a</mi></mtd></mtr><mtr><mtd><msup><mi>a</mi><mi>T</mi></msup></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mo>;</mo></mrow>最后，根据式(13)得到模型误差的估计值步骤四对补偿后的模型利用二阶插值滤波进行状态估计，得到卫星的姿态；具体为将步骤三中得到的模型误差代入状态方程(7)进行补偿，将补偿后的卫星姿态确定系统模型的状态方程和测量方程写成离散的非线性形式，为xk+1＝f(xk，dG，wk)(18)yk＝g(xk，vk)设滤波值的误差方差阵的平方根为即是的Cholesky分解，模型误差方差阵Q的平方根为Sw，测量噪声的方差阵R的平方根为Sv，状态预测误差的方差阵的平方根为即<mrow><mi>Q</mi><mo>=</mo><msub><mi>S</mi><mi>w</mi></msub><msubsup><mi>S</mi><mi>w</mi><mi>T</mi></msubsup><mo>,</mo></mrow><mrow><mi>R</mi><mo>=</mo><msub><mi>S</mi><mi>v</mi></msub><msubsup><mi>S</mi><mi>v</mi><mi>T</mi></msubsup><mo>,</mo></mrow>(19)<mrow><mover><mi>P</mi><mo>&OverBar;</mo></mover><mo>=</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>,</mo></mrow><mrow><mover><mi>P</mi><mo>^</mo></mover><mo>=</mo><msub><mover><mi>S</mi><mo>^</mo></mover><mi>x</mi></msub><msubsup><mover><mi>S</mi><mo>^</mo></mover><mi>x</mi><mi>T</mi></msubsup><mo>.</mo></mrow>状态及其误差方差阵的一步预测<msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn><mo>=</mo><mfrac><mrow><msup><mi>h</mi><mn>2</mn></msup><mo>-</mo><msub><mi>n</mi><mi>x</mi></msub><mo>-</mo><msub><mi>n</mi><mi>w</mi></msub></mrow><msup><mi>h</mi><mn>2</mn></msup></mfrac><mi>f</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow></mrow></msub><mrow><mo>+</mo><mfrac><mn>1</mn><mrow><mn>2</mn><msup><mi>h</mi><mn>2</mn></msup></mrow></mfrac><munderover><mi>Σ</mi><mrow><mi>p</mi><mo>=</mo><mn>1</mn></mrow><msub><mi>n</mi><mi>x</mi></msub></munderover><mrow><mo>(</mo><mi>f</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow><mo>+</mo><mi>f</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>20</mn><mo>)</mo></mrow></mrow><mrow><mo>+</mo><mfrac><mn>1</mn><mrow><mn>2</mn><msup><mi>h</mi><mn>2</mn></msup></mrow></mfrac><munderover><mi>Σ</mi><mrow><mi>p</mi><mo>=</mo><mn>1</mn></mrow><msub><mi>n</mi><mi>v</mi></msub></munderover><mrow><mo>(</mo><mi>f</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>,</mo></mrow></msub><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>w</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>)</mo></mrow><mo>+</mo><mi>f</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>,</mo></mrow></msub><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>w</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>)</mo></mrow></mrow><mo>)</mo></mrow>其中，表示w(k)的均值，nx表示状态向量的维数，nw表示系统噪声的维数，插值步长h2＝3；状态预测误差方差阵的Cholesky分解是经Householder变换后的矩阵<mrow><mover><mi>S</mi><mo>_</mo></mover><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><mo>[</mo><msubsup><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msubsup><mi>S</mi><mi>xw</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msubsup><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><msubsup><mi>S</mi><mi>xw</mi><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>]</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>21</mn><mo>)</mo></mrow></mrow>其中，<mrow><msubsup><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mo>{</mo><msubsup><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mrow><mo>(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo>)</mo></mrow></msub><mo>}</mo><mo>=</mo><mo>{</mo><mrow><mo>(</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>h</mi><mo>}</mo><mo>,</mo></mrow><mrow><msubsup><mi>S</mi><mi>xw</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mo>{</mo><msubsup><mi>S</mi><mi>xw</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mrow><mo>(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo>)</mo></mrow></msub><mo>}</mo><mo>=</mo><mo>{</mo><mrow><mo>(</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>w</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>w</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>h</mi><mo>}</mo><mo>,</mo></mrow><mrow><msubsup><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mo>{</mo><msubsup><mi>S</mi><mrow><mi>x</mi><mover><mi>x</mi><mo>^</mo></mover></mrow><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mrow><mo>(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo>)</mo></mrow></msub><mo>}</mo><mo>=</mo><mo>{</mo><mfrac><msqrt><msup><mi>h</mi><mn>2</mn></msup><mo>-</mo><mn>1</mn></msqrt><mrow><mn>2</mn><msup><mi>h</mi><mn>2</mn></msup></mrow></mfrac><mrow><mo>(</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow></mrow></mrow><mrow><mo>+</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow><mo>-</mo><mn>2</mn><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow><mo>)</mo><mo>}</mo><mo>,</mo></mrow><mrow><msubsup><mi>S</mi><mrow><mi>x</mi><mi>w</mi></mrow><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mo>=</mo><mo>{</mo><msubsup><mi>S</mi><mrow><mi>x</mi><mi>w</mi></mrow><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><msub><mrow><mo>(</mo><mi>k</mi><mo>)</mo></mrow><mrow><mo>(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo>)</mo></mrow></msub><mo>}</mo><mo>=</mo><mo>{</mo><mfrac><msqrt><msup><mi>h</mi><mn>2</mn></msup><mo>-</mo><mn>1</mn></msqrt><mrow><mn>2</mn><msup><mi>h</mi><mn>2</mn></msup></mrow></mfrac><mrow><mo>(</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>,</mo></mrow></msub><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>w</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow></mrow></mrow><mrow><mo>+</mo><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>,</mo></mrow></msub><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>^</mo></mover><mrow><mi>w</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>-</mo><mn>2</mn><msub><mi>f</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>,</mo><msub><mover><mi>d</mi><mo>^</mo></mover><mi>G</mi></msub><mo>,</mo><msub><mover><mi>w</mi><mo>&OverBar;</mo></mover><mi>k</mi></msub><mo>)</mo></mrow><mo>)</mo><mo>}</mo><mo>,</mo></mrow>其中，表示的第j列，表示的第j列，表示Sw的第j列；观测值的一步预测为<mrow><msub><mover><mi>y</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>=</mo><mfrac><mrow><msup><mi>h</mi><mn>2</mn></msup><mo>-</mo><msub><mi>n</mi><mi>x</mi></msub><mo>-</mo><msub><mi>n</mi><mi>v</mi></msub></mrow><msup><mi>h</mi><mn>2</mn></msup></mfrac><mi>g</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow></mrow><mrow><mo>+</mo><mfrac><mn>1</mn><mrow><mn>2</mn><msup><mi>h</mi><mn>2</mn></msup></mrow></mfrac><munderover><mi>Σ</mi><mrow><mi>p</mi><mo>=</mo><mn>1</mn></mrow><msub><mi>n</mi><mi>x</mi></msub></munderover><mrow><mo>(</mo><mi>g</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>+</mo><mi>g</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>22</mn><mo>)</mo></mrow></mrow><mrow><mo>+</mo><mfrac><mn>1</mn><mrow><mn>2</mn><msup><mi>h</mi><mn>2</mn></msup></mrow></mfrac><munderover><mi>Σ</mi><mrow><mi>p</mi><mo>=</mo><mn>1</mn></mrow><msub><mi>n</mi><mi>v</mi></msub></munderover><mrow><mrow><mo>(</mo><mi>g</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo></mrow></msub><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>v</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>)</mo></mrow><mo>+</mo><mi>g</mi><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo></mrow></msub><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>v</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>)</mo></mrow></mrow><mo>)</mo></mrow><mo></mo></mrow>其中，nv是测量噪声维数；的误差方差阵的Cholesky分解是Sy(k+1)经Householder变换后的矩阵<mrow><msub><mi>S</mi><mi>y</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><mo>[</mo><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>]</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>23</mn><mo>)</mo></mrow></mrow>其中，<mrow><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><mo>{</mo><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mrow><mo>(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo>)</mo></mrow></msub><mo>}</mo><mo>=</mo><mo>{</mo><mrow><mo>(</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>h</mi><mo>}</mo><mo>,</mo></mrow><mrow><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><mo>{</mo><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mrow><mo>(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo>)</mo></mrow></msub><mo>}</mo><mo>=</mo><mo>{</mo><mrow><mo>(</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo></mrow></msub><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>v</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>-</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo></mrow></msub><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>v</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>)</mo></mrow><mo>/</mo><mn>2</mn><mi>h</mi><mo>}</mo><mo>,</mo></mrow><mrow><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><mo>{</mo><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mrow><mo>(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo>)</mo></mrow></msub><mo>}</mo><mo>=</mo><mo>{</mo><mfrac><msqrt><msup><mi>h</mi><mn>2</mn></msup><mo>-</mo><mn>1</mn></msqrt><mrow><mn>2</mn><msup><mi>h</mi><mn>2</mn></msup></mrow></mfrac><mrow><mo>(</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow></mrow></mrow><mrow><mo>+</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>x</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>-</mo><mn>2</mn><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>)</mo><mo>}</mo><mo>,</mo></mrow><mrow><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><mo>{</mo><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mrow><mo>(</mo><mi>i</mi><mo>,</mo><mi>j</mi><mo>)</mo></mrow></msub><mo>}</mo><mo>=</mo><mo>{</mo><mfrac><msqrt><msup><mi>h</mi><mn>2</mn></msup><mo>-</mo><mn>1</mn></msqrt><mrow><mn>2</mn><msup><mi>h</mi><mn>2</mn></msup></mrow></mfrac><mrow><mo>(</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>+</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>v</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow></mrow></mrow><mrow><mo>+</mo><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn><mo>,</mo></mrow></msub><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>-</mo><mi>h</mi><msub><mover><mi>s</mi><mo>&OverBar;</mo></mover><mrow><mi>v</mi><mo>,</mo><mi>j</mi></mrow></msub><mo>)</mo></mrow><mo>-</mo><mn>2</mn><msub><mi>g</mi><mi>i</mi></msub><mrow><mo>(</mo><msub><mover><mi>x</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>,</mo><msub><mover><mi>v</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>)</mo><mo>}</mo><mo>,</mo></mrow>式中表示Sv的第j列；状态和测量的互协方差阵为<mrow><msub><mi>P</mi><mi>xy</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><msub><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow></msub><msup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mi>T</mi></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>24</mn><mo>)</mo></mrow></mrow>由式(24)得到增益矩阵Kk+1为Kk+1＝Pxy(k+1)[Sy(k+1)Sy(k+1)T]-1(25)基于测量的状态及估计误差方差阵更新为<mrow><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mover><mi>x</mi><mo>-</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mrow><mo>(</mo><msub><mi>y</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>-</mo><msub><mover><mi>y</mi><mo>&OverBar;</mo></mover><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>26</mn><mo>)</mo></mrow></mrow><mrow><mover><mi>P</mi><mo>^</mo></mover><mrow><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>=</mo><msup><mrow><mo>[</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>-</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><msubsup><mi>S</mi><mrow><mi>y</mi><mover><mi>x</mi><mo>&OverBar;</mo></mover></mrow><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>]</mo><mo>[</mo><msub><mover><mi>S</mi><mo>&OverBar;</mo></mover><mi>x</mi></msub><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>-</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><msubsup><mi>S</mi><mi>yx</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>]</mo></mrow><mi>T</mi></msup></mrow><mo>+</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><msup><mrow><mo>[</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>]</mo></mrow><mi>T</mi></msup></mrow><mrow><mo>+</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><msubsup><mi>S</mi><mi>yx</mi><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><msup><mrow><mo>[</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><msubsup><mi>S</mi><mi>yx</mi><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>]</mo></mrow><mi>T</mi></msup><mo>+</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><msup><mrow><mo>[</mo><msub><mi>K</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow></msub><msubsup><mi>S</mi><mi>yv</mi><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></msubsup><mrow><mo>(</mo><mi>k</mi><mo>+</mo><mn>1</mn><mo>)</mo></mrow><mo>]</mo></mrow><mi>T</mi></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>27</mn><mo>)</mo></mrow></mrow>由此得到状态估计值其中为卫星的姿态四元数，每次估计之后对姿态四元数进行归一化处理，并且按照式(28)(29)(30)解算得到姿态角；俯仰角θ＝-arcsin(2q1q3-2q2q4)(28)偏航角<mrow><mi>Ψ</mi><mo>=</mo><mi>arctan</mi><mrow><mo>(</mo><mfrac><mrow><msub><mn>2</mn><mrow><mi>q</mi><mn>2</mn><mi>q</mi><mn>3</mn></mrow></msub><mo>-</mo><msub><mn>2</mn><mrow><mi>q</mi><mn>1</mn><mi>q</mi><mn>4</mn></mrow></msub></mrow><mrow><msubsup><mi>q</mi><mn>3</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>q</mi><mn>4</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>1</mn><mn>2</mn></msubsup><mo>-</mo><msubsup><mi>q</mi><mn>2</mn><mn>2</mn></msubsup></mrow></mfrac><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>29</mn><mo>)</mo></mrow></mrow>滚转角FSA00000138460900014.tif,FSA00000138460900015.tif,FSA00000138460900016.tif,FSA00000138460900017.tif,FSA00000138460900018.tif,FSA00000138460900019.tif,FSA00000138460900024.tif,FSA00000138460900025.tif,FSA00000138460900026.tif,FSA00000138460900027.tif,FSA00000138460900028.tif,FSA000001384609000210.tif,FSA00000138460900032.tif,FSA00000138460900033.tif,FSA00000138460900035.tif,FSA00000138460900036.tif,FSA00000138460900037.tif,FSA000001384609000310.tif,FSA00000138460900041.tif,FSA00000138460900044.tif,FSA00000138460900045.tif,FSA00000138460900046.tif,FSA00000138460900048.tif,FSA00000138460900049.tif,FSA00000138460900051.tif,FSA00000138460900059.tif,FSA000001384609000510.tif,FSA000001384609000511.tif,FSA000001384609000512.tif,FSA000001384609000513.tif,FSA000001384609000514.tif,FSA000001384609000515.tif,FSA000001384609000516.tif,FSA000001384609000517.tif,FSA000001384609000518.tif,FSA00000138460900064.tif,FSA00000138460900065.tif,FSA000001384609000613.tif,FSA000001384609000614.tif,FSA000001384609000615.tif,FSA000001384609000616.tif,FSA000001384609000617.tif,FSA000001384609000621.tif,FSA00000138460900077.tif,FSA000001384609000712.tif,FSA000001384609000713.tif,FSA000001384609000715.tif全文摘要本发明公开了一种基于星敏感器和陀螺的高精度卫星姿态确定方法，包括以下几个步骤，步骤一建立卫星姿态确定系统的状态方程；步骤二建立卫星姿态确定系统的测量方程；步骤三利用预测滤波在线实时估计模型误差；步骤四对补偿后的模型利用二阶插值滤波进行状态估计，得到卫星的姿态。本发明采用了预测滤波在线实时估计模型误差并修正系统模型，克服了传统估计过程中将误差处理为零均值白噪声的缺点；而且可以处理任何非线性系统和噪声情况，获得更高精度的估计结果，适用于高精度姿态确定领域。文档编号G01C21/00GK101846510SQ201010194288公开日2010年9月29日申请日期2010年5月28日优先权日2010年5月28日发明者杨静,魏明坤申请人:北京航空航天大学

亚星游戏官网-www.yaxin868.com

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