小视场星敏感器量测延时滤波算法

钱华明， 王迪， 吴永慧， 黄智开

(哈尔滨工程大学 自动化学院， 哈尔滨 150001)

小视场(Narrow Filed of View,NFOV)星敏感器成本较低，并且在极端天气下更加适用，因此小视场星敏感器的姿态估计算法具有重要的研究价值。通过对导航星表进行预处理，导致星图识别过程中识别时间增加，影响星敏感器输出信息的时间，造成量测延时问题。同时，光学系统以及确定姿态等步骤的繁琐性，也导致了姿态敏感器输出信息的同时带有延时现象。

延时问题的存在，导致系统模型中存在了不确定性。鲁棒滤波算法被证实是一种处理模型不确定性问题的有效算法[1-3]。文献[4]讨论了针对有界模型不确定的线性时域系统二阶鲁棒卡尔曼滤波(Robust Kalman Filter，RKF)算法，但该模型不适用于非线性系统。文献[5]提出鲁棒扩展卡尔曼滤波(Robust Extended Kalman Filter，REKF)算法，通过预测-校正形式估计刚体姿态，所提出的姿态估计系统考虑四元数刚体模型，其建模时主要考虑的是加性噪声，但其不适于乘性耦合噪声的环境。文献[6]设计了一种RKF算法处理乘性耦合噪声，但其不能很好地解决量测延时带来的影响。文献[7]设计了一种RKF算法用于处理状态延迟与量测丢失的时变离散系统，通过求取状态估计协方差的最小上界来计算需要的参数。文献[8]提出了鲁棒有界时域滤波(Robust Finite Horizon Filter，RFHF)算法用于解决确定概率分布情况的延迟系统。但是文献[7-8]这2种算法只适用于线性系统。文献[9]提出了基于5阶球半径规则的容积卡尔曼滤波算法，使其更适用于非线性系统，解决传感器随机延时的问题，然而该算法在建模时没有考虑乘性耦合噪声，且与本文所研究的实际情况并不相符。

因此，针对小视场星敏感器与陀螺仪结合的模型，本文提出了REKF算法用于处理量测延时因素引起的模型不确定情况。通过状态扩维理论获得带有量测延时的非线性系统模型，并且建立的系统模型包含乘性耦合噪声。根据最小方差准则近似确定估计误差方差的最小上界范围，通过该最小上界选取滤波增益参数，从而得到量测延时情况下的REKF算法。仿真结果表明，本文算法所能达到的估计精度较原有算法有明显提高。

1 组合姿态算法设计

1.1 系统模型

1.1.1 陀螺的量测模型

在姿态估计系统中，陀螺仪用来测量卫星转过角速率的姿态敏感器。陀螺仪产生的误差因素有很多，如失准角误差、标度因素误差、漂移误差、时间延迟等。大部分的误差可以通过校正来补偿，而随机漂移、噪声因素等是主要的误差源，故陀螺量测模型可表示为[10]

(1)

1.1.2 星敏感器的量测模型

用四元数来表示量测模型为

pb=A(q)po+vs

(2)

1.2 状态方程与量测方程

由四元数轨道动力学方程可得

(3)

(4)

式中：

(5)

根据式(2)，选取星敏感器输出的姿态四元数作为量测变量，选取3个参考矢量描述姿态估计系统，故系统的量测模型为

(6)

由于星敏感器进行姿态估计时存在量测延时现象，上述建立的星敏感器量测模型没有考虑延时的存在，使得输出结果并不准确，笔者考虑一步延时的星敏感器量测模型：

yk=(Ι-Γk)zk+Γkzk-1

(7)

(8)

故根据式(4)、式(6)、式(7)可以得出关于量测延时及乘性噪声的非线性离散系统：

(9)

式中：Cik为有恰当维数的已知矩阵;ξik为均值为零、方差为1的噪声;vk为零均值高斯白噪声。由于实际的量测输出yk与zk、zk-1两个时刻的量测量均相关，需要利用状态扩维理论，使其得到相同时刻量测量的表达式。

(10)

式中：

(11)

式中：σ1、σ2为非负标量。

2 鲁棒扩展卡尔曼滤波算法

针对带有量测延时及乘性噪声的离散系统式(9)，经过状态扩维理论，获得一个相对简单的模型式(10)，提出改进的REKF算法，并且计算预测方差，给出估计误差方差的最小上界。

一步状态预测

(12)

状态更新

(13)

根据以上分析，对于带有乘性噪声及量测延时的不确定性系统，就是设计式(10)的REKF算法的问题。由于实际的误差方差难以求出，只能通过确定滤波增益参数来确保误差方差的最小上界，即

(14)

2.1 估计误差方差

在REKF算法中，通过选取一些模型不确定参数来表示其对系统的影响。与EKF算法类似，需要对状态函数与量测函数进行泰勒展开，主要区别在于根据上界范围进行滤波器的具体设计。通过这样的计算，使得求得的精度、效果等相较于EKF算法更优良。

一步状态预测误差可写为

(15)

(16)

(17)

将式(16)、式 (17)代入式(15)可得

(18)

由于ηik服从高斯分布，不同时刻之间是互相独立的，其相关性为0。而ηik与wk是互相独立的高斯白噪声，均值为零。故一步预测误差方差矩阵可表示为

BkβkLk)T]+E[g(Xk,ηk)gT(Xk,ηk)]+

(19)

式中：

(20)

(21)

式(19)可简化表示为

(22)

已知k+1时刻的状态估计误差与真实值和估计值有关，可以表示为

(23)

将式(10)代入式(23)中，可以得到

(24)

同式(16)相同，对量测函数泰勒展开，保留高阶项，并代入式(24)得

(25)

将式(25)代入式(24)中，所以有

Kk+1γk+1s(Xk+1,ξk+1)-Kk+1γk+1Vk+1-

(26)

将式(26)代到式(14)中，得到滤波误差方差矩阵，表示为

(27)

由式(22)、式(27)可以得到方差矩阵Pk+1|k、Pk+1，因为所建立的系统模型考虑了乘性噪声及量测延时等不确定性因素，其中包括未知矩阵如βk、αk+1，所以并不能实际求出协方差的具体值。根据最小方差准则的要求，求取预测误差方差Pk+1|k和滤波更新误差方差Pk+1的最小上界来近似替代。

2.2 算法设计

根据文献[14]中的2条引理(引理6.1、引理6.2)构造上界范围，并且可以求得滤波更新误差方差矩阵Pk+1的最小上界。

(28)

根据式(28)可以得到

(29)

E[g(Xk,ηk)gT(Xk,ηk)]≤

(30)

(Fk+BkβkLk)Pk(Fk+BkβkLk)T≤

(31)

将式(29)～式 (31)代入到式(22)中，可以求出一步预测误差方差矩阵的上界：

(32)

E(s(Xk+1,ξk+1)sT(Xk+1,ξk+1))=

(33)

将式(33)代入式(27)，得到滤波误差方差矩阵为

(34)

同理，根据二阶矩原理，假设存在正数ε2，同样能求解得到

(35)

由式(15)可得到

(36)

假设存在正数ε3，同样能求解得到

(37)

所以有

(38)

(39)

(40)

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

(41)

根据式(32)、式 (41)得到了一步预测方差和滤波更新方差的上界，当ε1、ε2、ε3、λ、μ都是正数时，提出以下2个离散Riccati方程：

(42)

(43)

用数学归纳法证明Σk+1|k、Σk+1是Pk+1|k、Pk+1的上界范围，即

(44)

在初始k=0时刻，能够轻易求出P0=Σ0>0，根据不等式(29)、式(30)以及等式(43)可得

P1|0≤Σ1|0

(45)

由式(45)及式(44)中的第一项可以得出

(46)

由式(45)和式(46)，可以推断出k=0时刻的状态误差方差满足：

P1≤Σ1

(47)

选取k=n-1时刻，假设满足不等式Pn≤Σn，那么当k=n时，同式(46)类似，有

(48)

因此，根据式(44)可以得出

(49)

(50)

(51)

设计滤波器的目的是根据状态模型和不同观测量中的信息计算状态变量的估计值，达到状态量的估计值与真实值误差最小，精度最高；在滤波设计过程中，需要权衡多种不同信息，充分合理地利用这些信息。REKF的设计是基于不确定的系统模型进行的，REKF算法的性能在于不确定性模型是否能完整描述实际系统。为了获得理想的滤波效果，式(1)给出的不确定性模型需要准确给出不确定性参数的大小。

3 仿真实验与分析

3.1 仿真环境

3.2 仿真分析

1) 情况1

从图1和图2可以看出，当假设系统不存在延时情况时，各滤波算法的性能相差不大，基本都能达到0.000 1°左右的精度。整体来看，REKF算法和RKF算法要略优于AEKF算法和RFHF算法，这是由于AEKF算法和RFHF算法在设计时都没有考虑到乘性噪声的影响，导致它们在非线性估计时存在一定误差，因此滤波精度稍差。

2) 情况2

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

从图3和图4可以看出，REKF算法处理带有延时及乘性噪声的非线性系统的效果要明显优于RFHF算法、RKF算法及AEKF算法，这是因为建立了带有乘性噪声和延时的误差模型来表示这种情况，而AEKF算法并不适用带有延时的系统，RKF算法只能满足乘性噪声项所带来的干扰，但没有考虑延时问题。所以，当出现延时时，RKF算法并不能保证系统精度，甚至会带来滤波发散。而RFHF算法只考虑了量测延时和量测丢失，没有考虑乘性噪声的影响，因此，在具有乘性噪声的实验环境下不能达到最理想的状态。

图1 情况1时姿态角均方根误差对比Fig.1 Comparison of root mean square error of attitude angle in Case 1

图2 情况1时姿态角误差对比Fig.2 Comparison of attitude angle error in Case 1

图3 情况2时姿态角均方根误差对比Fig.3 Comparison of RMSE of attitude angle in Case 2

图4 情况2时姿态角误差对比Fig.4 Comparison of attitude angle error in case 2

4 结 论

针对小视场星敏感器姿态估计时存在的信息延时问题，本文做了如下研究：

1) 建立带有延时不确定项的误差模型，该模型考虑到非线性系统同时存在乘性噪声及量测模型延时的情况，对REKF滤波算法进行改进。

2) 在算法设计时，根据最小均方误差准则要求，通过求取预测误差方差和滤波更新误差方差的最小上界进而确定滤波增益的最优值。REKF算法的性能在于不确定性模型来表示实际系统，从而达到状态估计误差最小，精度最高。

3) 仿真结果表明，REKF滤波算法可以有效解决量测延时问题，提高姿态估计的精度。