CN109000638A - 一种小视场星敏感器量测延时滤波方法 - Google Patents

Abstract

本发明公开了一种小视场星敏感器量测延时滤波方法，属于飞行器姿态估计技术领域。本发明考虑到非线性姿态估计系统状态模型存在乘性噪声以及量测模型存在未知干扰的情况，建立了两种模型不确定存在下的飞行器姿态估计模型，同时基于扩展卡尔曼滤波的结构，利用鲁棒算法设计找到预测方差及估计方差的上界范围，再利用最小方差理论设计最优的滤波增益。本发明采用鲁棒扩展卡尔曼滤波对飞行器姿态确定，其本质是一种最小方差准则估计，不同于一般的频率域滤波器，本发明可以用于处理不确定性条件下进行姿态估计，具有实时性高、对先验信息依赖度低等优势，解决了星敏感器存在延时的问题，即使在信息不确定的情况下也能保证较高的姿态估计精度。

Description

一种小视场星敏感器量测延时滤波方法

技术领域

本发明属于飞行器姿态估计技术领域，具体涉及一种利用鲁棒扩展卡尔曼滤波的小视场星敏感器量测延时滤波方法。

背景技术

由陀螺和星敏感器组合的姿态估计系统由于定姿精度高被广泛地应用于飞行器中。欧拉角、修正罗德里格斯参数、方向余弦、四元数等是飞行器的主要姿态描述参数。四元数由于计算简单，无三角函数的运算，同时又能避免欧拉角的奇异性问题，因此，四元数常被作为姿态估计系统中的姿态描述参数。针对该系统的四元数姿态估计模型，许多基于扩展卡尔曼滤波(Extended Kalman Filter，EKF)的姿态估计方法被提出，如加性扩展卡尔曼滤波(Additive Extended Kalman Filter，AEKF)和乘性扩展卡尔曼滤波(MultiplicativeExtended Kalman Filter，MEKF)。然而，扩展卡尔曼滤波仅仅适合于带加性噪声的精确已知的系统模型。如果系统模型中存在模型不确定的情况，该算法的性能将会受到严重的影响。因此，许多带模型不确定的非线性滤波算法被发展，如H_∞滤波，集值非线性滤波，鲁棒滤波设计等，其中，基于最小方差的鲁棒滤波设计被证明是一种用于解决系统带模型不确定情况的有效处理手段，然而大多数基于最小方差的鲁棒滤波都是针对系统只存在一种模型不确定的情况。如果系统中存在两种或是两种以上的模型不确定，上述的鲁棒滤波算法将会失效。

针对姿态估计系统，系统状态方程中存在状态与高斯白噪声耦合的乘性噪声，该噪声的方差未知，因此被看成是一种状态模型不确定，一种鲁棒扩展卡尔曼滤波(RobustExtended Kalman Filter,REKF)被提出。由于小视场星敏感器独有的优势将其用于研究小视场下姿态估计算法中。视场减小使得星敏感器捕获星点数目减少，导致星图识别准确率大幅下降，影响后续姿态估计的精度。为了解决星点丢失，我们对导航星表进行预处理，使得小视场星敏感器对暗星的探测能力增强。我们需要选取不同的星等的星点进行导航星表构建，构建的导航星表相比大视场星敏感器的星表要大，所以在星图识别匹配阶段时，星图识别的时间增加导致了延时，影响姿态信息输出的实时性。同时，由于光学系统以及确定姿态等步骤的繁琐性，也导致了姿态敏感器输出信息的同时带有延时现象。针对小视场星敏感器的特点，将陀螺仪与星敏感器组合进行姿态估计，同时考虑星图识别及数据处理中不可避免的延时现象，建立带有量测信息延时的不确定性系统误差模型。基于求取最小误差方差准则的鲁棒滤波算法被证实是一种处理模型不确定时有效算法，故在鲁棒滤波的基础上，给出一种改进的鲁棒扩展滤波算法(REKF)处理量测延迟的问题。当构建的导航星表内满足星图识别的最低三颗星数的要求后，通过鲁棒扩展卡尔曼滤波算法来解决量测延时问题；若构建的导航星表内仍不满足最低三颗星数的要求，即用鲁棒扩展卡尔曼滤波算法来解决量测延时问题。

发明内容

本发明的目的在于提供一种提高姿态估计的精度和增强系统的鲁棒性的小视场星敏感器量测延时滤波方法。

本发明的目的是这样实现的：

本发明公开了一种小视场星敏感器量测延时滤波方法，包括以下步骤：

(1)采集飞行器运动过程中陀螺与星敏感器的输出数据；

(2)建立量测延时下基于姿态四元数的飞行器非线性状态空间模型，包括建立飞行器姿态估计系统的状态方程及量测延时下飞行器姿态估计系统的量测方程；

(3)针对上述量测延时下的飞行器状态空间模型，已知k时刻的状态估计值和方差的上界，以扩展卡尔曼滤波为结构框架，利用鲁棒扩展卡尔曼滤波设计，进行时间更新，求得一步状态预测和预测方差的上界；

(4)利用上述求得的一步状态预测和预测方差的上界，进行鲁棒扩展卡尔曼滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，同时由于四元数归一化约束，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；

(5)姿态估计系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺漂移的结果，完成姿态估计；若k<M，表示滤波过程未完成，则重复步骤(3)至步骤(4)，直至姿态估计系统运行结束。

对于一种小视场星敏感器量测延时滤波方法，所述的步骤(2)具体包括以下步骤：

(2.1)建立飞行器姿态估计系统的状态方程：

将姿态四元数q_k和陀螺漂移β_k组成维数为n的状态变量根据飞行器的运动学方程，建立基于姿态四元数的飞行器状态方程为：

式中：w_k满足均值为零，方差为的高斯白噪声；是乘性噪声项，s选取3，η_i,k表示均值为零，方差为1的噪声，A_ik表示有适当阶数的确定矩阵，为：

式中：σ_v为陀螺测量噪声；

(2.2)建立量测延时下飞行器姿态估计系统的量测方程：

式中：z_k为k时刻星敏感器量测值；r_k为星敏感器的参考矢量；i为星敏感器观测到恒星的个数；v_k为零均值的高斯白噪声，其方差为A(q_k)为四元数q_k的姿态描述矩阵，若A(q_k)能表示为：

由于星敏感器进行姿态估计时，不可避免的存在量测延迟现象，上述建立的星敏感器量测模型没有考虑延迟的存在，使得输出结果并不准确，我们考虑一步延时的情况给出带有延迟的星敏感器测量模型：

y_k＝(Ι-Γ_k)z_k+Γ_kz_k

y_k表示k时刻真实的量测量，Γ_k表示不同的延迟速率，满足Γ_k＝diag{[μ_k,1μ_k,2....μ_k,m]}，其中μ_k,i∈R(i＝1,2...m)服从伯努利分布(0～1分布)，满足：

关于量测延时的非线性离散系统：

由于实际的量测输出y_k与z_k、z_k-1两个时刻的量测量均相关，需要利用状态扩维理论，使其得到相同时刻量测量的表达式：

式中，是扩维后的状态变量，是扩维后的状态函数，表示扩维后的乘性噪声项，表示扩维后的高斯白噪声，表示扩维后的量测函数，表示扩维后的量测噪声，γ_k＝[Ι-Γ_kΓ_k]表示扩维后的量测延时项，

其中，由Γ_k的分布特性可知，是一个零均值的随机矩阵序列；h(x_k)满足：

σ₁、σ₂是标量。

优选的，所述的步骤(2)中陀螺测量噪声标准差为陀螺漂移噪声标准差为陀螺的采样周期为Δt＝0.25s；星敏感器测量噪声标准差为σ_s＝0.005deg，输出频率为1Hz；星敏感器的参考矢量设为r₁＝[100]^Τ，r₂＝[010]^Τ，r₃＝[001]^Τ；初始陀螺漂移β₀＝[0.10.10.1]^Tdeg/h；σ_ij＝10sec(i＝1,2,3；j＝x,y,z)。

对于一种小视场星敏感器量测延时滤波方法，所述的步骤(3)具体包括：

假设k时刻的状态估计值为方差上界为P_k，根据鲁棒扩展卡尔曼滤波，求得一步状态预测值为

一步状态预测误差为：

扩维后的状态函数f'(X_k)通过在点处泰勒展开，得到：

式中， 表示经过展开后的高阶项，通过引入未知时变矩阵β_k∈R^n×n，满足具体的缩放矩阵B_k∈R^n×n，L_k表示已知的调节矩阵，故高阶项可表示为：

一步预测误差方差矩阵可表示为：

式中：

而η_ik与w_k是互相独立的高斯白噪声，均值为零；故：Ε[W_kg^T(X_k,η_k)]、

这几项都是零矩阵；

则一步预测方差可简化表示为：

根据二阶矩的概念可以得到：

假设存在正数λ和矩阵L_k，满足条件能推导出：

求得一步预测误差方差矩阵的上界：

优选的，所述的步骤(3)中初始状态估计值为x₀＝[0 0 0 1 0.1 0.1 0.1]^T；忽略非线性函数线性化的高阶项，B_k，D_k+1设置为0；调节矩阵为了保证系统能达到的精度，参数λ和μ均设置为0.0001，参数ε设置为0.1；延时速率设为

对于一种小视场星敏感器量测延时滤波方法，所述的步骤(4)具体包括：

同EKF算法中量测更新步骤类似，对量测函数泰勒展开，保留高阶项得到：

式中，α_k+1是未知的时变矩阵，D_k+1是考虑不确定系统模型误差的比例矩阵；

将式(23)代入一步状态预测值中，所以有：

将上式代到滤波误差协方差矩阵，表示为：

其中， 表示为：

同样根据二阶矩原理，假设存在正数ε₂、ε₃：

假设存在正数μ和矩阵L_k+1，满足条件能推出：

所以，k+1时刻的更新后状态误差的方差矩阵为：

利用最小方差理论，令求得最优滤波增益为：

最后利用求得的最优滤波增益求出k+1时刻的状态估计值和方差的上界P_k+1；

由于状态变量四元数部分满足四元数归一化约束||q_k||＝1，因此再将k+1时刻的状态估计值中的四元数部分进行归一化约束。

优选的，所述的步骤(4)中C_ik＝0。

优选的，所述的步骤(5)中，M＝1000。

本发明的有益效果在于：

本发明考虑到非线性姿态估计系统状态模型存在乘性噪声以及量测模型存在未知干扰的情况，建立了两种模型不确定存在下的飞行器姿态估计模型，同时基于扩展卡尔曼滤波的结构，利用鲁棒算法设计找到预测方差及估计方差的上界范围，再利用最小方差理论设计最优的滤波增益。

本发明同时考虑到非线性姿态估计系统状态模型存在乘性噪声以及量测模型存在未知干扰的情况，有利于提高系统的鲁棒性；

本发明采用基于最小方差的鲁棒滤波设计，能够实现在最小方差意义下最优滤波增益设计，有利用提高系统的姿态估计精度。

本发明创新性的采用鲁棒扩展卡尔曼滤波对飞行器姿态确定，其本质是一种最小方差准则估计，不同于一般的频率域滤波器，本发明可以用于处理不确定性条件下进行姿态估计，具有实时性高、对先验信息依赖度低等优势，解决了星敏感器存在延时的问题，即使在信息不确定的情况下也能保证较高的姿态估计精度。

附图说明

图1为本发明中小视场星敏感器量测延时滤波方法的步骤示意图；

图2为本发明中小视场星敏感器量测延时滤波方法的流程图；

图3为实施例1中姿态角均方根误差对比；

图4为实施例1中各姿态角误差对比；

图5为实施例2中姿态角均方根误差对比；

图6为为实施例2中各姿态角误差对比。

具体实施方式

下面结合附图对本发明做进一步描述。

结合图1及图2，本发明公开了一种小视场星敏感器量测延时滤波方法，包括以下步骤：

步骤一：采集飞行器运动过程中陀螺与星敏感器的输出数据；

步骤二：建立量测延时下基于姿态四元数的飞行器非线性状态空间模型；

步骤2.1建立飞行器姿态估计系统的状态方程；

将姿态四元数q_k和陀螺漂移β_k组成维数为n的状态变量根据飞行器的运动学方程，建立基于姿态四元数的飞行器状态方程为：

式中：w_k满足均值为零，方差为的高斯白噪声。是乘性噪声项，s选取3，η_i,k表示均值为零，方差为1的噪声，A_ik表示有适当阶数的确定矩阵，为：

式中：σ_v为陀螺测量噪声；

步骤2.2建立量测延时下飞行器姿态估计系统的量测方程；

式中：z_k为k时刻星敏感器量测值；r_k为星敏感器的参考矢量；i为星敏感器观测到恒星的个数；v_k为零均值的高斯白噪声，其方差为A(q_k)为四元数q_k的姿态描述矩阵，若A(q_k)能表示为：

由于星敏感器进行姿态估计时，不可避免的存在量测延迟现象，上述建立的星敏感器量测模型没有考虑延迟的存在，使得输出结果并不准确，我们考虑一步延时的情况给出带有延迟的星敏感器测量模型：

y_k＝(Ι-Γ_k)z_k+Γ_kz_k (5)

y_k表示k时刻真实的量测量，Γ_k表示不同的延迟速率，满足Γ_k＝diag{[μ_k,1μ_k,2....μ_k,m]}，其中μ_k,i∈R(i＝1,2...m)服从伯努利分布(0～1分布)，满足：

关于量测延时的非线性离散系统：

由于实际的量测输出y_k与z_k、z_k-1两个时刻的量测量均相关，需要利用状态扩维理论，使其得到相同时刻量测量的表达式：

式中，是扩维后的状态变量，是扩维后的状态函数，表示扩维后的乘性噪声项，表示扩维后的高斯白噪声，表示扩维后的量测函数，表示扩维后的量测噪声，γ_k＝[Ι-Γ_kΓ_k]表示扩维后的量测延时项，

其中，由Γ_k的分布特性可知，是一个零均值的随机矩阵序列。h(x_k)满足：

σ₁、σ₂是标量。

步骤三：针对上述量测延时下的飞行器状态空间模型，已知k时刻的状态估计值和方差的上界，以扩展卡尔曼滤波为结构框架，利用鲁棒扩展卡尔曼滤波设计，进行时间更新，求得一步状态预测和预测方差的上界；

假设k时刻的状态估计值为方差上界为P_k，根据鲁棒扩展卡尔曼滤波，求得一步状态预测值为

一步状态预测误差为：

扩维后的状态函数f'(X_k)通过在点处泰勒展开，得到：

式中， 表示经过展开后的高阶项，通过引入未知时变矩阵β_k∈R^n×n，满足具体的缩放矩阵B_k∈R^n×n，L_k表示已知的调节矩阵，故高阶项可表示为：

一步预测误差方差矩阵可表示为：

式中：

而η_ik与w_k是互相独立的高斯白噪声，均值为零。故：Ε[W_kg^T(X_k,η_k)]、 这几项都是零矩阵。

那么一步预测方差可简化表示为：

根据二阶矩的概念可以得到：

假设存在正数λ和矩阵L_k，满足条件能推导出：

求得一步预测误差方差矩阵的上界：

步骤四：利用上述求得的一步状态预测和预测方差的上界，进行鲁棒扩展卡尔曼滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，同时由于四元数归一化约束，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；

同EKF算法中量测更新步骤类似，对量测函数泰勒展开，保留高阶项得到：

式中，α_k+1是未知的时变矩阵，D_k+1是考虑不确定系统模型误差的比例矩阵。将式(23)代入一步状态预测值中，所以有：

将上式代到滤波误差协方差矩阵，表示为：

其中， 表示为：

同样根据二阶矩原理，假设存在正数ε₂、ε₃：

假设存在正数μ和矩阵L_k+1，满足条件能推出：

所以，k+1时刻的更新后状态误差的方差矩阵为：

利用最小方差理论，令求得最优滤波增益为：

最后将上述求得的最优滤波增益代入到式(24)和式(28)中，求出k+1时刻的状态估计值和方差的上界P_k+1；

由于状态变量四元数部分满足四元数归一化约束||q_k||＝1，因此再将k+1时刻的状态估计值中的四元数部分进行归一化约束；

步骤五：姿态估计系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺漂移的结果，完成姿态估计；若k<M，表示滤波过程未完成，则重复步骤三至步骤四，直至姿态估计系统运行结束。

对于一种小视场星敏感器量测延时滤波方法，步骤二中，陀螺测量噪声标准差为陀螺漂移噪声标准差为陀螺的采样周期为Δt＝0.25s；星敏感器测量噪声标准差为σ_s＝0.005deg，输出频率为1Hz；星敏感器的参考矢量设为r₁＝[1 0 0]^Τ，r₂＝[0 1 0]^Τ，r₃＝[0 0 1]^Τ；初始陀螺漂移β₀＝[0.1 0.1 0.1]^Tdeg/h；σ_ij＝10sec(i＝1,2,3；j＝x,y,z)；

对于一种小视场星敏感器量测延时滤波方法，步骤三中，初始状态估计值为x₀＝[0 00 1 0.1 0.1 0.1]^T；忽略非线性函数线性化的高阶项，B_k，D_k+1设置为0；调节矩阵为了保证系统能达到的精度，参数λ和μ均设置为0.0001，参数ε设置为0.1。延时速率设为

对于一种小视场星敏感器量测延时滤波方法，步骤四中，C_ik＝0。

对于一种小视场星敏感器量测延时滤波方法，步骤五中，M＝1000。

为了验证本发明的合理性、可行性，利用Matlab程序对设计的小视场星敏感器量测延时滤波方法进行仿真，为实现对比分析，将本发明方法与AEKF、RKF方法进行比较。

综合考虑各种情况，设定了两个实施例：

实施例1

初始状态值为x₀＝[0 0 0 1 0.1 0.1 0.1]^T，对应的初始四元数部分为q₀＝[0 00 1]^T，初始陀螺漂移部分为β₀＝[0.1 0.1 0.1]^Tdeg/h，星敏感器的三个参考矢量设为r₁＝[1 0 0]^Τ，r₂＝[0 1 0]^Τ，r₃＝[0 0 1]^Τ。星敏感器与陀螺的采样频率为1Hz。初始的三个姿态角误差方差矩阵表示为(0.1deg)²Ι_3×3，陀螺漂移的误差方差矩阵表示为(0.2deg/h)²Ι_3×3。实际的星敏感器精度较高，我们可以把泰勒展开得到的高阶项忽略，B_k，D_k+1设置为0。调节矩阵为了保证系统能达到的精度，参数λ和μ均设置为0.0001，参数ε设置为0.1。通过第二章给出的状态模型及量测模型可知，其中包括加性噪声，并且我们考虑噪声及延迟对系统的影响，可令矩阵C_ik＝0。量测模型参数σ_ij＝10sec(i＝1,2,3；j＝x,y,z)，仿真时间设为两小时。

实施例2

假设星敏感器工作时出现延时现象，设三个方向的矢量延时速率各不相同，并且满足伯努利分布，有：

仿真结果：

结合图2及图3，当假设系统不存在延时情况时，各滤波算法的性能相差不大，基本都能达到0.0001deg左右的精度。整体来看，REKF和RKF滤波算法要略优于AEKF滤波算法，这是因为AEKF算法经过非线性估计后存在一定的误差，导致其滤波精度稍差一点。

结合图4及图5，我们提出的鲁棒扩展卡尔曼滤波算法(REKF)处理带有延时及乘性噪声的非线性系统的效果要明显优于RKF滤波算法及AEKF滤波算法，这是因为我们建立了带有延时的误差模型来表示这种情况，而AEKF滤波算法并不适用带有延时的系统，RKF滤波算法只能满足乘性噪声项所带来的干扰，但没有考虑延时问题。所以，当出现延时时，RKF算法并不能保证系统精度，甚至会带来滤波发散。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种小视场星敏感器量测延时滤波方法，其特征在于，包括以下步骤：

(1)采集飞行器运动过程中陀螺与星敏感器的输出数据；

(2)建立量测延时下基于姿态四元数的飞行器非线性状态空间模型，包括建立飞行器姿态估计系统的状态方程及量测延时下飞行器姿态估计系统的量测方程；

(3)针对上述量测延时下的飞行器状态空间模型，已知k时刻的状态估计值和方差的上界，以扩展卡尔曼滤波为结构框架，利用鲁棒扩展卡尔曼滤波设计，进行时间更新，求得一步状态预测和预测方差的上界；

(4)利用上述求得的一步状态预测和预测方差的上界，进行鲁棒扩展卡尔曼滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，同时由于四元数归一化约束，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；

(5)姿态估计系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺漂移的结果，完成姿态估计；若k<M，表示滤波过程未完成，则重复步骤(3)至步骤(4)，直至姿态估计系统运行结束。

2.根据权利要求1所述的一种小视场星敏感器量测延时滤波方法，其特征在于，所述的步骤(2)具体包括以下步骤：

(2.1)建立飞行器姿态估计系统的状态方程：

将姿态四元数q_k和陀螺漂移β_k组成维数为n的状态变量根据飞行器的运动学方程，建立基于姿态四元数的飞行器状态方程为：

式中：w_k满足均值为零，方差为的高斯白噪声；是乘性噪声项，s选取3，η_i,k表示均值为零，方差为1的噪声，A_ik表示有适当阶数的确定矩阵，为：

式中：σ_v为陀螺测量噪声；

(2.2)建立量测延时下飞行器姿态估计系统的量测方程：

式中：z_k为k时刻星敏感器量测值；r_k为星敏感器的参考矢量；i为星敏感器观测到恒星的个数；v_k为零均值的高斯白噪声，其方差为A(q_k)为四元数q_k的姿态描述矩阵，若A(q_k)能表示为：

由于星敏感器进行姿态估计时，不可避免的存在量测延迟现象，上述建立的星敏感器量测模型没有考虑延迟的存在，使得输出结果并不准确，我们考虑一步延时的情况给出带有延迟的星敏感器测量模型：

y_k＝(Ι-Γ_k)z_k+Γ_kz_k

y_k表示k时刻真实的量测量，Γ_k表示不同的延迟速率，满足Γ_k＝diag{[μ_k,1 μ_k,2 ....μ_k,m]}，其中μ_k,i∈R(i＝1,2...m)服从伯努利分布(0～1分布)，满足：

关于量测延时的非线性离散系统：

由于实际的量测输出y_k与z_k、z_k-1两个时刻的量测量均相关，需要利用状态扩维理论，使其得到相同时刻量测量的表达式：

式中，是扩维后的状态变量，是扩维后的状态函数，表示扩维后的乘性噪声项，表示扩维后的高斯白噪声，表示扩维后的量测函数，表示扩维后的量测噪声，γ_k＝[Ι-Γ_k Γ_k]表示扩维后的量测延时项，

其中，由Γ_k的分布特性可知，是一个零均值的随机矩阵序列；h(x_k)满足：

σ₁、σ₂是标量。

3.根据权利要求2所述的一种小视场星敏感器量测延时滤波方法，其特征在于：所述的步骤(2)中陀螺测量噪声标准差为陀螺漂移噪声标准差为陀螺的采样周期为Δt＝0.25s；星敏感器测量噪声标准差为σ_s＝0.005deg，输出频率为1Hz；星敏感器的参考矢量设为r₁＝[1 0 0]^Τ，r₂＝[0 1 0]^Τ，r₃＝[0 0 1]^Τ；初始陀螺漂移β₀＝[0.1 0.1 0.1]^Tdeg/h；σ_ij＝10sec(i＝1,2,3；j＝x,y,z)。

4.根据权利要求1所述的一种小视场星敏感器量测延时滤波方法，其特征在于，所述的步骤(3)具体包括：

假设k时刻的状态估计值为方差上界为P_k，根据鲁棒扩展卡尔曼滤波，求得一步状态预测值为

一步状态预测误差为：

扩维后的状态函数f'(X_k)通过在点处泰勒展开，得到：

式中， 表示经过展开后的高阶项，通过引入未知时变矩阵β_k∈R^n×n，满足具体的缩放矩阵B_k∈R^n×n，L_k表示已知的调节矩阵，故高阶项可表示为：

一步预测误差方差矩阵可表示为：

式中：

而η_ik与w_k是互相独立的高斯白噪声，均值为零；故：Ε[W_kg^T(X_k,η_k)]、 这几项都是零矩阵；

则一步预测方差可简化表示为：

根据二阶矩的概念可以得到：

假设存在正数λ和矩阵L_k，满足条件能推导出：

求得一步预测误差方差矩阵的上界：

5.根据权利要求4所述的一种小视场星敏感器量测延时滤波方法，其特征在于：所述的步骤(3)中初始状态估计值为x₀＝[0 0 0 1 0.1 0.1 0.1]^T；忽略非线性函数线性化的高阶项，B_k，D_k+1设置为0；调节矩阵为了保证系统能达到的精度，参数λ和μ均设置为0.0001，参数ε设置为0.1；延时速率设为

6.根据权利要求1所述的一种小视场星敏感器量测延时滤波方法，其特征在于，所述的步骤(4)具体包括：

同EKF算法中量测更新步骤类似，对量测函数泰勒展开，保留高阶项得到：

式中， α_k+1是未知的时变矩阵，D_k+1是考虑不确定系统模型误差的比例矩阵；

将上式代入一步状态预测值中，所以有：

将上式代到滤波误差协方差矩阵，表示为：

其中， 表示为：

同样根据二阶矩原理，假设存在正数ε₂、ε₃：

假设存在正数μ和矩阵L_k+1，满足条件能推出：

所以，k+1时刻的更新后状态误差的方差矩阵为：

利用最小方差理论，令求得最优滤波增益为：

最后利用求得的最优滤波增益求出k+1时刻的状态估计值和方差的上界P_k+1；

由于状态变量四元数部分满足四元数归一化约束||q_k||＝1，因此再将k+1时刻的状态估计值中的四元数部分进行归一化约束。

7.根据权利要求6所述的一种小视场星敏感器量测延时滤波方法，其特征在于：所述的步骤(4)中C_ik＝0。

8.根据权利要求1所述的一种小视场星敏感器量测延时滤波方法，其特征在于：所述的步骤(5)中，M＝1000。