一种基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法

1.本发明涉及航天器姿态估计技术领域，具体涉及一种基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法。


背景技术：

2.高精度高可靠的姿态确定是航天器开展空间在轨服务等任务的基础。目前航天器的姿态确定方法按姿态解算方法不同可以分为确定性方法和状态估计法两类，其中，状态估计法采用滤波的方法根据观测信息对航天器状态量进行估计，可以有效的克服参考矢量的不确定性影响。
3.在高斯噪声假设下，航天器姿态确定主要应用扩展卡尔曼滤波和sigma点卡尔曼滤波，基于最小均方误差准则的经典卡尔曼滤波和基于最小均方误差准则的各类非线性滤波器在高斯噪声的条件下有着最优或近似最优的滤波效果。然而，在实际工程实践中往往会出现非高斯噪声，在面对工程中经常出现的野值、脉冲等诱导的非高斯噪声时，卡尔曼滤波不具有鲁棒性，而基于最小均方误差准则的各类非线性滤波器也必须满足高斯假设，在面对非高斯环境时会出现精度差甚至滤波发散的现象。
4.为处理非高斯噪声，目前主要使用非高斯滤波器，非高斯滤波器包括：粒子滤波器(pf)、高斯和滤波器(gsp)、huber滤波器、student’s t滤波器、最大相关熵滤波器(mckf)和最小误差熵滤波器(meekf)。其中，粒子滤波器采用序贯重要性采样方法来近似计算后验密度，可以处理任意非高斯噪声；高斯和滤波器将非高斯噪声建模为高斯和分布以处理非高斯噪声；huber滤波器采用m估计的方法处理非高斯噪声，有着较好的鲁棒性；student’s t滤波器将非高斯噪声假设为student’s t分布以处理非高斯噪声；最大相关熵滤波器和最小误差熵滤波器分别以最大相关熵准则和最小误差熵准则为最优准则，相比传统的最小均方误差准则有着更好的非高斯噪声的处理效果。
5.然而，上述的非高斯滤波器中，粒子滤波器具有较高的计算复杂度，且其粒子退化和粒子贫化问题也是很难处理的问题；高斯和滤波器假设噪声的概率密度是已知的，且有着较大的计算量；huber滤波器的精度有限；student’s t滤波器只能对特定的非高斯噪声进行处理，环境适应性较差；最大相关熵滤波器虽然可以将概率密度函数的峰值固定到零，但是单独使用时算法的精度有限；最小误差熵滤波器具有平移不变性，不能保证估计误差收敛到零。


技术实现要素：

6.为解决上述现有技术中存在的部分或全部技术问题，本发明提供一种基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法。
7.本发明的技术方案如下：
8.提供了一种基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法，所述方法用于确定航天器姿态，所述方法包括：
9.根据航天器的观测数据和航天器姿态动力学模型，建立航天器姿态确定的非线性系统；
10.根据所述非线性系统和泰勒展开公式，获取航天器对应的状态预测方程和近似观测方程，并对状态预测方程和近似观测方程进行增广处理；
11.基于增广处理后的状态预测方程和近似观测方程，构建中心误差熵准则滤波的代价函数，对所述代价函数进行最大化处理，确定航天器状态变量的最优估计值。
12.在一些可选的实施方式中，建立航天器姿态确定的非线性系统为：
[0013][0014]
其中，x
k
表示k时刻航天器的n维状态变量，f(
·
)表示系统的状态方程，y
k
表示k时刻的m维观测向量，h(
·
)表示系统的量测方程，w
k
‑1表示k
‑
1时刻的n维系统噪声序列，v
k
表示k时刻的m维观测噪声序列。
[0015]
在一些可选的实施方式中，根据所述非线性系统和泰勒展开公式，进行一步预测和泰勒展开，得到如以下公式七和公式九的航天器对应的状态预测方程和近似观测方程；
[0016][0017][0018]
其中，表示k时刻的一步预测状态估计值，表示k
‑
1时刻的状态估计值，h
k
表示非线性函数h(
·
)的雅克比矩阵。
[0019]
在一些可选的实施方式中，利用以下公式十一对状态预测方程和近似观测方程进行增广处理；
[0020][0021]
其中，i
n
表示n维单位矩阵，
[0022]
在一些可选的实施方式中，构建中心误差熵准则滤波的代价函数为：
[0023][0024]
其中，λ表示权重系数，l＝m+n，表示核宽为σ1的高斯核函数，e
i
表示误差变量e的第i维状态，表示核宽为σ2的高斯核函数，e
j
表示误差变量e的第j维状态。
[0025]
在一些可选的实施方式中，利用以下公式十六确定航天器状态变量x
k
的最优估计值
[0026][0027]
其中，
[0028]
在一些可选的实施方式中，所述对所述代价函数进行最大化处理，确定航天器状态变量的最优估计值，包括：
[0029]
对代价函数计算梯度，将梯度计算公式表达成矩阵形式，并令梯度等于0；
[0030]
基于矩阵形式的梯度计算公式，采用定点迭代算法，确定航天器状态变量的最优估计值计算公式；
[0031]
基于航天器状态变量的最优估计值计算公式，根据矩阵求逆引理公式，确定航天器状态变量的最优估计值。
[0032]
本发明技术方案的主要优点如下：
[0033]
本发明的基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法在航天器姿态确定过程中，将航天器姿态确定系统的状态方程和观测方程重新改写为非线性回归模型，然后通过最大化中心误差熵准则函数来求解最优的状态估计，能够提高处理非高斯噪声时的航天器姿态估计精度和鲁棒性。
附图说明
[0034]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0035]
图1为本发明一实施例的基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法的流程图；
[0036]
图2为在传统扩展卡尔曼滤波算法下的航天器姿态确定结果示意图；
[0037]
图3为在传统最大相关熵扩展卡尔曼滤波算法下的航天器姿态确定结果示意图；
[0038]
图4为在传统最小误差熵扩展卡尔曼滤波算法下的航天器姿态确定结果示意图；
[0039]
图5为在本发明一实施例的基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法下的航天器姿态确定结果示意图。
具体实施方式
[0040]
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例及相应的附图对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0041]
以下结合附图，详细说明本发明实施例提供的技术方案。
[0042]
参见图1，本发明一实施例提供了一种基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法，该方法用于确定航天器姿态，包括以下步骤：
[0043]
根据航天器的观测数据和航天器姿态动力学模型，建立航天器姿态确定的非线性
系统；
[0044]
根据非线性系统和泰勒展开公式，获取航天器对应的状态预测方程和近似观测方程，并对状态预测方程和近似观测方程进行增广处理；
[0045]
基于增广处理后的状态预测方程和近似观测方程，构建中心误差熵准则滤波的代价函数，对代价函数进行最大化处理，确定航天器状态变量的最优估计值。
[0046]
以下对本发明一实施例提供的基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法的步骤及原理进行具体说明。
[0047]
具体地，根据航天器的观测数据和航天器姿态动力学模型，建立航天器姿态确定的非线性系统为：
[0048][0049]
其中，x
k
表示k时刻航天器的n维状态变量，f(
·
)表示系统的状态方程，y
k
表示k时刻的m维观测向量，h(
·
)表示系统的量测方程，w
k
‑1表示k
‑
1时刻的n维系统噪声序列，v
k
表示k时刻的m维观测噪声序列，且w
k
和v
k
互不相关。
[0050]
假设：w
k
和v
k
满足公式二，航天器初始状态x0与w
k
和v
k
相独立，航天器初始状态x0的均值和协方差如公式三所示；
[0051][0052][0053]
其中，e(
·
)表示数学期望，q
k
表示系统噪声w
k
的协方差矩阵，r
k
表示观测噪声v
k
的协方差矩阵，δ
kj
表示kronecker
‑
δ函数，cov(
·
)表示协方差，表示x0对应的初始估计值，p0表示x0对应的初始协方差。
[0054]
将非线性函数f(x
k
‑1)和h(x
k
)展开成泰勒级数，并略去二阶以上项，得到公式四和公式五；
[0055][0056][0057]
其中，表示k
‑
1时刻的状态估计值，表示k时刻的一步预测状态估计值，f
k
‑1和h
k
分别表示非线性函数f(
·
)和h(
·
)的雅克比矩阵，f
k
‑1和h
k
的形式如公式六所示；
[0058][0059]
进一步地，基于上述的航天器姿态确定的非线性系统，进行一步预测得到航天器对应的状态预测方程，其中，状态预测方程包括状态一步预测方程，状态一步预测方程和协方差一步预测方程如公式七和公式八所示；
[0060]
[0061][0062]
其中，p
k|k
‑1表示k时刻的一步预测状态协方差矩阵，p
k
‑1表示k
‑
1时刻的状态协方差矩阵，q
k
‑1表示w
k
‑1的协方差矩阵。
[0063]
根据泰勒展开可知，观测方程的近似，即近似观测方程为：
[0064][0065]
设定：公式十，利用以下公式十一对上述得到的状态预测方程和近似观测方程进行增广处理；
[0066][0067]
其中，i
n
表示n维单位矩阵，且μ
k
满足以下公式十二；
[0068][0069]
其中，s
k
、s
p,kk
‑1和s
r,k
分别表示矩阵p
k|k
‑1和r
k
的cholesky分解。
[0070]
对上述公式十一的等式两边同乘得到：
[0071]
d
k
＝w
k
x
k
+e
k
公式十三
[0072]
其中，其中，
[0073]
其中，可将d
k
、w
k
和e
k
分别表示为d
k
＝[d
1,k
,d
2,k
,
…
,d
l,k
]
t
、w
k
＝[w
1,k
,w
2,k
,
…
,w
l,k
]
t
和e
k
＝[e
1,k
,e
2,k
,
…
,e
l,k
]
t
，l＝m+n，d
l,k
表示d
k
的第l个元素，w
l,k
表示w
k
的第l行向量，e
l,k
表示e
k
的第l个元素。
[0074]
进一步地，基于增广处理后的状态预测方程和近似观测方程，构建中心误差熵准则滤波(ceekf)的代价函数为：
[0075][0076]
其中，λ表示权重系数，表示核宽为σ1的高斯核函数，e
i
表示误差变量e的第i维状态，表示核宽为σ2的高斯核函数，e
j
表示误差变量e的第j维状态。
[0077]
设定：则公式十四可以表述为：
[0078][0079]
在中心误差熵(centered error entropy，cee)准则下，航天器状态变量x
k
的最优估计值可以通过最大化上述构建的代价函数得到，具体地，航天器状态变量x
k
的最优估计值可以通过以下公式十六确定；
[0080][0081]
其中，表示j
l
(x
k
)取得最大值时对应的x
k
值。
[0082]
进一步地，对代价函数进行最大化处理，确定航天器状态变量的最优估计值，可以包括以下步骤：
[0083]
对代价函数计算梯度，将梯度计算公式表达成矩阵形式，并令梯度等于0；
[0084]
基于矩阵形式的梯度计算公式，采用定点迭代算法，确定航天器状态变量的最优估计值计算公式；
[0085]
基于航天器状态变量的最优估计值计算公式，根据矩阵求逆引理公式，确定航天器状态变量的最优估计值。
[0086]
具体地，可以利用以下公式十七对代价函数计算梯度，并令梯度等于0；
[0087][0088]
其中，其中，
[0089]
将上述公式十七所示的代价函数梯度计算公式表达成如以下公式十八所示的矩阵形式；
[0090][0091]
其中，其中，其中，表示φ
k
的第i行第j列元素，
[0092]
根据公式十三，可以得到e
k
＝d
k
‑
w
k
x
k
，基于矩阵形式的代价函数梯度计算公式，采用定点迭代算法，得到航天器状态变量x
k
的最优估计值计算公式为：
[0093][0094]
其中，表示第t+1次迭代的状态值。
[0095]
进行如以下公式二十至公式二十七的设定：
[0096][0097][0098][0099][0100]
其中，λ
x,k
表示n
×
n维矩阵，λ
xy,k
为m
×
n维矩阵，λ
yx,k
为n
×
m维矩阵，λ
y,k
为m
×
m维矩阵，λ
x,k
、λ
xy,k
、λ
yx,k
和λ
y,k
的表达式如以下公式二十四、公式二十五、公式二十六和公式二十七所示；
[0101]
[0102][0103][0104][0105]
进行如以下公式二十八的设定：
[0106][0107]
基于上述设定，将μ
k
表示为：
[0108][0109]
将表示为：
[0110][0111]
将表示为：
[0112][0113]
进行如以下公式三十二至公式三十四的设定：
[0114][0115][0116]
a3＝h公式三十四
[0117]
基于上述设定，可以将航天器状态变量x
k
的最优估计值表示为：
[0118][0119]
根据矩阵求逆引理公式：(a+bcd)
‑1＝a
‑1‑
a
‑1b(c
‑1+dab)
‑1da
‑1，设定：a1＝a,a2＝b,i＝c,a3＝d；
[0120]
可以得到航天器状态变量x
k
的最优估计值为：
[0121][0122]
其中，其中，
[0123]
利用公式三十六获取的为航天器状态变量x
k
的后验估计值，即最终的最优估计值。
[0124]
相应的，航天器状态变量x
k
的后验协方差矩阵p
k
可以更新为：
[0125][0126]
其中，i表示单位矩阵。
[0127]
本发明一实施例提供的基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法在航天器姿态确定过程中，将航天器姿态确定系统的状态方程和观测方程重新改写为非线性回归模型，然后通过最大化中心误差熵准则函数来求解最优的状态估计，能够提高处理非高斯噪声时的航天器姿态估计精度和鲁棒性。
[0128]
以下结合具体示例，对本发明一实施例提供的基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法的有益效果进行说明。
[0129]
某航天器姿态确定系统的系统状态方程和观测方程如公式三十八所示：
[0130][0131]
其中，q
bo
为四元数表示的航天器姿态，b为陀螺仪的常值漂移，也作为状态变量被估计，ω
g
为陀螺仪测量值，ω
g
为输入，η
g
和η
b
为零均值系统噪声，η
g
和η
b
的协方差为q
g
和q
b
，q
opt
为航天器姿态敏感器的观测输出，q
n
为观测噪声四元数，ω
oi
＝[0
ꢀ‑
ω
0 0]表示惯性系下的轨道角速度，e
bo
表示从轨道系到星体系的坐标转换矩阵，e
bo
可表示为：
[0132][0133]
用于航天器姿态确定的参数设定如下：
[0134]
陀螺常值漂移：b＝[30 30 30]
t
(
°
)/h，常值漂移白噪声均方差σ
b
＝0.5(
°
)/h，陀螺测量噪声σ
g
＝0.5(
°
)/h，滤波初值选取为：x0＝[06×1]，p0＝diag(δq2i3×
3 δb2i3×3)，δq＝0.02，δb＝0.1(
°
)/h，ω
bo
＝10
‑4×
[cos(10ω0t) cos(8ω0t) cos(5.7ω0t)]，轨道角速度ω0＝0.0012rad/s，q
bo
(0)＝[1 0 0 0]
t
。
[0135]
假设航天器系统噪声为高斯噪声η
g
～n(0,q
g
)，η
b
～n(0,q
b
)，姿态敏感器观测噪声
为混合高斯噪声，
[0136]
附图2至附图5给出了利用传统扩展卡尔曼滤波(ekf)、最大相关熵扩展卡尔曼滤波(mcekf)、最小误差熵扩展卡尔曼滤波(meeekf)和本发明一实施例提供的基于中心误差熵扩展卡尔曼滤波的航天器姿态确定方法得到的不同姿态确定结果。附图中，rmse of：滚转角均方根误差，rmse ofθ：偏航角均方根误差，rmse ofψ：俯仰角均方根误差，time：时间。可以看出，本发明一实施例提供的基于中心误差熵准则扩展卡尔曼滤波的姿态确定方法具有最高的滤波精度和最好的滤波稳定性，能够更好地应对非高斯噪声，估计精度高，鲁棒性好。
[0137]
需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。此外，本文中“前”、“后”、“左”、“右”、“上”、“下”均以附图中表示的放置状态为参照。
[0138]
最后应说明的是：以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。