一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法与流程

1.本发明属于航空航天系统姿态估计领域，涉及一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法。


背景技术：

2.近年来，随着航空航天系统任务的快速发展，不断涌现的新型应用场景和载荷任务对飞行器本体姿态确定方法的动态估计精度、实时性能以及自适应能力提出了更高要求。卡尔曼滤波是广泛应用于航空航天和机器人等领域的姿态估计方法，近十年来研究人员研究提出了基于飞行器动力学和运动学李群对称不变特性的不变卡尔曼滤波方法，利用李群李代数变换简化系统模型以提高飞行器姿态估计性能。但是，在实际航空航天工程中，飞行器大幅动作所导致的重尾噪声会破坏不变卡尔曼滤波理论的最优性前提，进而影响飞行器姿态估计的实际性能。
3.所提出的分层高斯状态空间模型采用变分贝叶斯迭代计算飞行器的李群旋转矩阵，提高滤波算法对重尾过程噪声干扰的适应能力，是一种改善飞行器姿态估计性能的有效方法。


技术实现要素：

4.针对重尾噪声干扰下的不变卡尔曼滤波姿态估计，本发明提出一种分层高斯状态空间模型：基于t分布定义状态预测用到的概率密度函数，其中，尺度矩阵和自由度参数的共轭先验分布分别服从威希特和伽马分布；之后利用变分贝叶斯迭代，计算得到飞行器的尺度矩阵、自由度参数以及李群旋转矩阵。本发明所述方法对不变卡尔曼滤波器的不足进行了改进，解决了航空航天工程中飞行器姿态模型的在重尾噪声干扰下的姿态估计问题。
5.本发明采用的技术方案为：
6.一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，该方法根据观测数据为姿态估计过程中用到的分布参数分别定义其概率密度函数，之后采用变分贝叶斯迭代同时计算尺度矩阵、参数自由度以及李群旋转矩阵，显著提高了不变卡尔曼滤波的适应性和精度。具体包括以下步骤：
7.步骤一：建立矩阵李群so(3)上姿态估计的离散姿态动力学模型；
8.so(3)上姿态估计的离散姿态动力学模型如下所示：
9.r
k
＝exp
g
(w
k
‑1)r
k
‑1ω
k
‑
1 k＝1,2,...
ꢀꢀꢀ
(1)
[0010][0011]
其中，r
k
∈so(3)，表示在第k时刻将本体坐标系映射到地球地固坐标系的旋转矩阵；ω
k
∈g为根据姿态演变模型编码对瞬时旋转的控制输入；高斯白过程噪声
表示通过李群指数项exp
g
(w
k
)注入的小过程噪声，q
w
表示协方差；y
k
是具有已知向量b
′
和b
″
(例如重力和磁场)的离散噪声观测y
k
′
和y
k
″
的合成；v
k
′
,v
k
″
为独立的各向同性白噪声，其协方差为对角矩阵，分别用q
v
′
和q
v
″
表示。
[0012]
步骤二：将系统的李群动态模型映射到欧氏几何空间；
[0013]
设是r
k
‑1在k
‑
1时刻的估计值，是r
k
的估计值。定义不变误差ξ
k
与模型参数ω
k
‑1无关，其演化模型服从：
[0014]
ξ
k
＝ξ
k
‑1‑
w
k
o(|ξ
k
‑1|2,|w
k
|2,|ξ
k
‑1||w
k
|)
ꢀꢀꢀ
(3)
[0015]
其中，o(
·
)为关于ξ
k
‑1和w
k
的可忽略的二次以上小项。进一步忽略该二次项，则在李群动态模型下旋转矩阵r
k
的求解就可以近似转化为在欧式空间中线性状态空间下对不变误差ξ
k
的求解：
[0016][0017]
那么，观测序列y
k
则被重写为一个关于不变误差ξ
k
的函数，新的序列表示为z
k
，具有以下形式：
[0018][0019]
其中，和分别是映射后观测矩阵与噪声。
[0020]
步骤三：基于观测数据定义重尾噪声中各参数的先验分布函数；
[0021]
航天任务中，由于剧烈机动而导致较大的异常值，可以认为重尾噪声服从t分布：
[0022][0023]
其中，p(w
k
)表示过程噪声w
k
的概率密度；表示一个t分布，其均值为μ，尺度矩阵为σ，自由度为τ；γ
w
是一个辅助随机变量；γ(γ；α,β)代表一个标量γ的伽马分布概率密度函数，该标量分别以α和β作为形状与速率参数。为正态分布符号。
[0024]
重尾噪声的尺度矩阵服从逆威希特分布，这种分布常被用作高斯变量协方差矩阵的共轭先验。那么在姿态估计中，尺度矩阵的逆威希特概率密度函数可以被写作：
[0025][0026]
其中iw表示逆威希特分布；λ
k
表示自由度；ψ
k
是一个3
×
3的尺度矩阵的逆矩阵；|
·
|表示求行列式的操作；γ3(
·
)是三变量的伽马分布；
[0027]
重尾噪声的自由度参数τ
k
为伽马分布：
[0028]
p(τ
k
)＝γ(τ
k
；a
k
,b
k
)
ꢀꢀꢀ
(8)
[0029]
其中，a
k
,b
k
分别表示τ
k
伽马分布的形状与速率参数。
[0030]
通过式(6)～(8)为重尾干扰下的姿态估计问题建立起了一个基于t分布的分层高斯状态空间模型，为接下来的步骤打下基础。
[0031]
步骤四：对重尾噪声的概率密度函数进行变分贝叶斯近似；
[0032]
基于变分贝叶斯迭代，对重尾噪声后验概率密度函数的准确解析表达式进行分解，获得其分解的近似值：
[0033]
p(ξ
k
,γ
k
,σ
k∣k
‑1τ
k
∣z
1:k
)≈q(ξ
k
)q(γ
k
)q(σ
k∣k
‑1)q(τ
k
)
ꢀꢀꢀ
(9)
[0034]
其中，q(ξ
k
),q(γ
k
),q(σ
k∣k
‑1),q(τ
k
)分别表示ξ
k
,γ
k
,σ
k∣k
‑1,τ
k
的后验概率密度函数，p(ξ
k
,γ
k
,σ
k∣k
‑1τ
k
∣z
1:k
)的值通过对因子近似式和准确解析表达式之间的相对熵进行最小化来确定：
[0035][0036]
其中表示q(x)与p(x)之间的相对熵。根据变分贝叶斯迭代，以上近似后验q(ξ
k
),q(γ
k
),q(σ
k∣k
‑1),q(τ
k
)满足以下等式：
[0037][0038][0039][0040][0041]
其中，e
x
[
·
]表示对除x以外的所有变量求数学期望；c
x
是与x有关的常数。
[0042]
步骤五：定点迭代求解系统状态与概率密度函数的分布参数；
[0043]
基于ξ
k
与γ
k
,σ
k∣k
‑1,τ
k
三个参数的条件独立性，式(11)～(14)改写为以下等式：
[0044][0045]
其中，为转换后的测量方差矩阵，constant表示不影响后续迭代计算的常参数。
[0046]
根据定点迭代法求解ξ
k
,γ
k
,σ
k∣k
‑1,τ
k
四个参数。
[0047]
求解不变误差ξ
k
：
[0048][0049]
式中，迭代后验估计与后验协方差可以通过以下等式计算：
[0050]
[0051][0052][0053]
式(18)中的表示第i 1次迭代的校准卡尔曼增益矩阵，为i 1次迭代的先验估计协方差。
[0054]
求解辅助随机变量γ
k
：
[0055]
q
(i 1)
(γ
k
)＝γ(γ
k
；η
(i 1)
,θ
(i 1)
)
ꢀꢀꢀ
(20)
[0056]
式中，η
(i 1)
与θ
(i 1)
分别表示变量γ
k
伽马分布的形状与速率参数，分别通过下列公式计算：
[0057][0058][0059]
式中，为偏差量的均值估计，为偏差量的估计协方差。
[0060]
求解协方差矩阵σ
k∣k
‑1的先验估计：
[0061][0062]
由下列公式给出：
[0063][0064][0065]
式中，为中间参数。
[0066]
求解参数τ
k
的先验估计：
[0067][0068]
由下列公式给出：
[0069][0070][0071]
步骤六：将步骤五中求解出的各个参数带入步骤四中进行计算，得到噪声的后验概率密度函数近似值；
[0072]
根据式(20),(23),(26)，得出ξ
k
,γ
k
,σ
k∣k
‑1,τ
k
四个变量的期望计算公式：
[0073][0074]
[0075][0076][0077]
式中，ψ(
·
)为双伽玛函数。
[0078]
经过n次定点迭代，ξ
k
的后验概率密度函数就被近似为：
[0079][0080]
其中，σ
k∣k
分别是不变误差和误差协方差的后验估计。
[0081]
旋转状态r
k
的分布改写为集中高斯分布，其均值为协方差为σ
k∣k
：
[0082][0083][0084]
至此，计算出经过n次定点迭代后迭代更新得到姿态旋转矩阵的后验估计即可确定重尾噪声干扰环境下该时刻飞行器的旋转姿态矩阵。
[0085]
本发明的有益效果：本发明建立分层高斯分布状态空间模型，提出了一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法。由于该技术手段基于历史观测数据为过程噪声预设概率密度函数，并通过贝叶斯迭代对模型的参数进行求解，解决了重尾噪声干扰下的飞行器姿态估计问题，可以显著提高飞行器李群位姿估计问题中ikf对重尾噪声滤波的鲁棒性。
附图说明
[0086]
图1为本发明所述方法的流程框图。
[0087]
图2为本发明所提出rsikf滤波方法在不同n值下的armse结果。
[0088]
图3为α＝5,β＝10时各滤波方法的rmse结果。
[0089]
图4为α＝20,β＝10时各滤波方法的rmse结果。
具体实施方式
[0090]
以下结合附图对本发明作进一步详细的说明。这些附图均为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
[0091]
参照图1，一种基于信息反馈不变卡尔曼滤波器的自适应姿态估计方法，包括以下步骤：
[0092]
s1.建立矩阵李群so(3)上姿态估计的姿态动力学模型
[0093]
so(3)上的离散姿态动力学模型，如下所示：
[0094]
r
k
＝exp
g
(w
k
‑1)r
k
‑1ω
k
‑
1 k＝1,2,...
ꢀꢀꢀ
(1)
[0095][0096]
s2.将李群系统模型变换为李代数并转换到欧氏几何空间
[0097]
不变误差ξ
k
与模型参数ω
k
‑1无关，即：
[0098][0099]
重新定义并线性化创新序列：
[0100][0101]
s3：基于观测数据定义重尾噪声中各参数的先验分布函数
[0102]
设系统的过程噪声服从以下t分布：
[0103][0104]
尺度矩阵服从逆威希特分布：
[0105][0106]
自由度参数τ
k
定义为伽马分布：
[0107]
p(τ
k
)＝γ(τ
k
；a
k
,b
k
)
ꢀꢀꢀ
(7)
[0108]
步骤四：后验概率密度函数的变分贝叶斯近似
[0109]
对重尾噪声后验概率密度函数的准确解析表达式进行分解，获得其分解的近似值：
[0110]
p(ξ
k
,γ
k
,σ
k∣k
‑1τ
k
∣z
1:k
)≈q(ξ
k
)q(γ
k
)q(σ
k∣k
‑1)q(τ
k
)
ꢀꢀꢀ
(8)
[0111]
以上四个后验概率密度函数满足以下等式：
[0112][0113][0114][0115][0116]
步骤五：定点迭代求系统状态与分布参数
[0117]
基于ξ
k
与γ
k
,σ
k∣k
‑1,τ
k
三个参数的条件独立性，式(11)～(14)可以被改写为以下等式：
[0118][0119]
根据用定点迭代法分别求取ξ
k
,γ
k
,σ
k∣k
‑1,τ
k
四个参数。
[0120]
求解不变误差ξ
k
：
[0121][0122][0123][0124][0125]
求解辅助随机变量γ
k
：
[0126]
q
(i 1)
(γ
k
)＝γ(γ
k
；η
(i 1)
,θ
(i 1)
)
ꢀꢀꢀ
(18)
[0127][0128][0129]
求解协方差矩阵σ
k∣k
‑1的先验估计：
[0130][0131][0132][0133]
求解参数τ
k
的先验估计：
[0134][0135][0136][0137]
步骤六：后验概率密度函数的变分贝叶斯近似计算
[0138]
ξ
k
,γ
k
,σ
k∣k
‑1,τ
k
四个变量的期望计算公式：
[0139][0140]
[0141][0142][0143]
经过n次定点迭代，ξ
k
的后验概率密度函数就可以被近似为：
[0144][0145]
其中，σ
k∣k
分别是不变误差和误差协方差的后验估计。
[0146]
那么，旋转状态r
k
的分布可改写为集中高斯分布，其均值为协方差为σ
k∣k
：
[0147][0148][0149]
实施例：
[0150]
采用本发明提出的一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，给定系统参数和初始值为：
[0151]
σ
0|0
＝0.52362i3×3,σ
w
＝0.017452i3×3,b'＝[1,0,0]
t
,b”＝[0,1,0]
t
,σ
v'
＝0.08732i3×3,σ
v”＝0.08732i3×3.
[0152]
滤波参数设置为λ
k
＝k 4,a
k
＝2,b
k
＝6,n＝10.
[0153]
真实姿态轨迹时长5000秒，重尾过程噪声与重尾异常值一起生成：
[0154][0155]
并且为了比较理论性能，使用5000次随机运行的李代数中的误差变量来计算5000s滤波后的均方根误差和平均均方根误差：
[0156][0157][0158]
本例中，分别取α＝5,β＝10与α＝20,β＝10两种条件，对不同滤波方法在重尾噪声干扰下的性能进行模拟与比较，结果分别如图3、图4所示。
[0159]
由上述仿真结果可以看出，相比于传统ikf、hkf等滤波方法，本发明所提出的rsikf方法在以上实验条件下显著减小了滤波结果的均方差、平均均方差。因此，本发明所述的方法可以有效的解决重尾噪声干扰下的飞行器李群姿态估计问题，提高姿态估计的可靠性。
[0160]
以上是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，凡是依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均属于发明技术
方案的范围内。