本发明属于航天器自主运行,尤其涉及一种基于状态约束流形的解析化滤波方法。
背景技术:
1、航天器自主运行过程中所处环境复杂多变,其导航状态可能会受到过程噪声影响,而且航天器携带的导航敏感器也存在测量噪声,此类随机误差严重影响着航天器状态估计精度。导航滤波算法对观测数据、动力学模型等信息源进行处理,通过对随机误差的抑制,以实现高精度的状态估计。传统最优滤波需要进行高维矩阵求逆等复杂运算,难以在星上实施,只能采用基于飞行包络拟合的定增益矩阵滤波修正方法。这严重制约了现有航天自主导航系统的估计精度。
技术实现思路
1、本发明的技术解决问题:克服现有技术的不足,提供一种基于状态约束流形的解析化滤波方法,利用轨道动力学存在二次型约束的条件,以系统可观测度为泛函指标,通过约束优化和代数几何理论,在提升系统可观测度的同时实现了状态约束流形下的解析式状态估计。
2、为了解决上述技术问题,本发明公开了一种基于状态约束流形的解析化滤波方法,包括:
3、确定航天器的状态量x;其中,r和v分别表示航天器的位置和速度;
4、确定不考虑观测修正和状态约束时状态量x对应的先验估计以及考虑观测修正和状态约束时状态量x对应的先验估计则,基于无偏估计准则,有:
5、
6、其中,k表示待求取的滤波增益矩阵;∈表示测量新息;
7、建立泛函指标j:
8、
9、其中,tr表示矩阵的迹,e表示矩阵的期望,λ表示拉格朗日算子,a表示约束矩阵,l表示常值标量;
10、将公式(4)代入到公式(5)中,有:
11、
12、其中,r表示测量噪声的协方差矩阵,i表示单位矩阵,h表示测量矩阵,p-表示协方差矩阵;
13、根据最优必要条件,有:
14、
15、其中,w表示测量值的预测协方差矩阵;
16、根据公式(8),有:
17、
18、其中,ε表示中间变量,d表示状态修正量;
19、将公式(9)代入公式(4)、然后再代入公式(3),得到λ的解;
20、进行矩阵的对角化分解:
21、
22、其中,v表示特征矩阵,γ表示对角矩阵,u表示正交矩阵,ξ表示对角矩阵;
23、将公式(9)代入到公式(8)中,有:
24、
25、其中,和均为中间变量;
26、确定可观测度解析指标v与协方差矩阵p-的映射关系:
27、
28、其中,表示系统lie导数对位置的偏导数,表示系统lie导数对速度的偏导数;
29、将公式(13)代入到公式(11)中,求解得到:
30、
31、其中,表示的第(i,j)个元素,表示的第(i,j)个元素,γj表示对角矩阵γ的第j个对角线元素,εi表示ξ的第i个对角线元素;
32、求得将求得的k代入到公式(4)中,完成状态的测量更新。
33、在上述基于状态约束流形的解析化滤波方法中,系统状态方程表示如下:
34、
35、其中,f(·)表示状态方程。
36、在上述基于状态约束流形的解析化滤波方法中,系统观测方程表示如下:
37、y=h(x)+η…(2)
38、其中,h(·)表示测量方程,η表示观测噪声;y表示观测量,
39、在上述基于状态约束流形的解析化滤波方法中,
40、在上述基于状态约束流形的解析化滤波方法中,状态量x满足如下的约束流形:
41、xtax=l···(3)
42、其中,
43、在上述基于状态约束流形的解析化滤波方法中,根据最优必要条件,有:
44、
45、也即:
46、
47、对公式(7)进行简化,可得:
48、
49、其中,w=hp-ht+r。
50、在上述基于状态约束流形的解析化滤波方法中,d=p-htw-1∈;ε=∈tw-1∈。
51、在上述基于状态约束流形的解析化滤波方法中,γ=diag([0,…,0,λε])。
52、在上述基于状态约束流形的解析化滤波方法中,
53、
54、
55、其中,c为中间变量,
56、在上述基于状态约束流形的解析化滤波方法中,
57、
58、本发明具有以下优点:
59、(1)本发明公开了一种基于状态约束流形的解析化滤波方法,不同于传统卡尔曼滤波方法中通过矩阵求逆得到增益矩阵,本发明利用状态约束流形实现了增益矩阵的解析化表示,通过实现增益矩阵解析化,大大降低了滤波算法的计算复杂度,满足了星上计算需求。
60、(2)本发明公开了一种基于状态约束流形的解析化滤波方法,突破了星上传统的定增益矩阵滤波修正方法,有效抑制了随机误差对航天器自主导航精度的影响。
1.一种基于状态约束流形的解析化滤波方法,其特征在于,包括:
2.根据权利要求1所述的基于状态约束流形的解析化滤波方法,其特征在于,系统状态方程表示如下:
3.根据权利要求1所述的基于状态约束流形的解析化滤波方法,其特征在于,系统观测方程表示如下:
4.根据权利要求3所述的基于状态约束流形的解析化滤波方法,其特征在于,
5.根据权利要求1所述的基于状态约束流形的解析化滤波方法,其特征在于,状态量x满足如下的约束流形:
6.根据权利要求1所述的基于状态约束流形的解析化滤波方法,其特征在于,根据最优必要条件,有:
7.根据权利要求1所述的基于状态约束流形的解析化滤波方法,其特征在于,d=p-htw-1∈;ε=∈tw-1∈。
8.根据权利要求1所述的基于状态约束流形的解析化滤波方法,其特征在于,γ=diag([0,…,0,λε])。
9.根据权利要求1所述的基于状态约束流形的解析化滤波方法,其特征在于,
10.根据权利要求1所述的基于状态约束流形的解析化滤波方法,其特征在于,