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

Abstract

本发明提供一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法。包括以下步骤：步骤一、建立系统在李群特殊正交群上的姿态动力学模型；步骤二、将系统李群动态模型映射到欧氏几何空间；步骤三、基于观测数据定义重尾噪声中个参数的先验分布函数；步骤四、对重尾噪声的概率密度函数进行变分贝叶斯近似；步骤五、定点迭代求解系统状态与概率密度函数的分布参数；步骤六、将步骤五中的各个参数带入步骤四中进行计算，得到噪声的后验概率密度函数近似值。该方法很好地解决了系统在重尾噪声干扰下的李群姿态估计问题。

Description

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

技术领域

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

背景技术

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

所提出的分层高斯状态空间模型采用变分贝叶斯迭代计算飞行器的李群旋转矩阵，提高滤波算法对重尾过程噪声干扰的适应能力，是一种改善飞行器姿态估计性能的有效方法。

发明内容

针对重尾噪声干扰下的不变卡尔曼滤波姿态估计，本发明提出一种分层高斯状态空间模型：基于T分布定义状态预测用到的概率密度函数，其中，尺度矩阵和自由度参数的共轭先验分布分别服从威希特和伽马分布；之后利用变分贝叶斯迭代，计算得到飞行器的尺度矩阵、自由度参数以及李群旋转矩阵。本发明所述方法对不变卡尔曼滤波器的不足进行了改进，解决了航空航天工程中飞行器姿态模型的在重尾噪声干扰下的姿态估计问题。

本发明采用的技术方案为：

一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，该方法根据观测数据为姿态估计过程中用到的分布参数分别定义其概率密度函数，之后采用变分贝叶斯迭代同时计算尺度矩阵、参数自由度以及李群旋转矩阵，显著提高了不变卡尔曼滤波的适应性和精度。具体包括以下步骤：

步骤一：建立矩阵李群SO(3)上姿态估计的离散姿态动力学模型；

SO(3)上姿态估计的离散姿态动力学模型如下所示：

R_k＝exp_G(w_k-1)R_k-1Ω_k-1 k＝1,2,... (1)

其中，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″表示。

步骤二：将系统的李群动态模型映射到欧氏几何空间；

设是R_k-1在k-1时刻的估计值，/>是R_k的估计值。定义不变误差ξ_k与模型参数Ω_k-1无关，其演化模型服从：

ξ_k＝ξ_k-1-w_k+O(|ξ_k-1|²,|w_k|²,|ξ_k-1||w_k|) (3)

其中，O(·)为关于ξ_k-1和w_k的可忽略的二次以上小项。进一步忽略该二次项，则在李群动态模型下旋转矩阵R_k的求解就可以近似转化为在欧式空间中线性状态空间下对不变误差ξ_k的求解：

那么，观测序列Y_k则被重写为一个关于不变误差ξ_k的函数，新的序列表示为z_k，具有以下形式：

其中，和/>分别是映射后观测矩阵与噪声。

步骤三：基于观测数据定义重尾噪声中各参数的先验分布函数；

航天任务中，由于剧烈机动而导致较大的异常值，可以认为重尾噪声服从T分布：

其中，p(w_k)表示过程噪声w_k的概率密度；表示一个T分布，其均值为μ，尺度矩阵为Σ，自由度为τ；γ_w是一个辅助随机变量；Γ(γ；α,β)代表一个标量γ的伽马分布概率密度函数，该标量分别以α和β作为形状与速率参数。/>为正态分布符号。

重尾噪声的尺度矩阵服从逆威希特分布，这种分布常被用作高斯变量协方差矩阵的共轭先验。那么在姿态估计中，尺度矩阵的逆威希特概率密度函数可以被写作：

其中IW表示逆威希特分布；λ_k表示自由度；Ψ_k是一个3×3的尺度矩阵的逆矩阵；|·|表示求行列式的操作；γ₃(·)是三变量的伽马分布；

重尾噪声的自由度参数τ_k为伽马分布：

p(τ_k)＝Γ(τ_k；a_k,b_k) (8)

其中，a_k,b_k分别表示τ_k伽马分布的形状与速率参数。

通过式(6)～(8)为重尾干扰下的姿态估计问题建立起了一个基于T分布的分层高斯状态空间模型，为接下来的步骤打下基础。

步骤四：对重尾噪声的概率密度函数进行变分贝叶斯近似；

基于变分贝叶斯迭代，对重尾噪声后验概率密度函数的准确解析表达式进行分解，获得其分解的近似值：

p(ξ_k,γ_k,Σ_k∣k-1τ_k∣z_1:k)≈q(ξ_k)q(γ_k)q(Σ_k∣k-1)q(τ_k) (9)

其中，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)的值通过对因子近似式和准确解析表达式之间的相对熵进行最小化来确定：

其中表示q(x)与p(x)之间的相对熵。根据变分贝叶斯迭代，以上近似后验q(ξ_k),q(γ_k),q(Σ_k∣k-1),q(τ_k)满足以下等式：

其中，E_x[·]表示对除x以外的所有变量求数学期望；c_x是与x有关的常数。

步骤五：定点迭代求解系统状态与概率密度函数的分布参数；

基于ξ_k与γ_k,Σ_k∣k-1,τ_k三个参数的条件独立性，式(11)～(14)改写为以下等式：

其中，为转换后的测量方差矩阵，constant表示不影响后续迭代计算的常参数。

根据定点迭代法求解ξ_k,γ_k,Σ_k∣k-1,τ_k四个参数。

求解不变误差ξ_k：

式中，迭代后验估计与后验协方差/>可以通过以下等式计算：

式(18)中的表示第i+1次迭代的校准卡尔曼增益矩阵，/>为i+1次迭代的先验估计协方差。

求解辅助随机变量γ_k：

q⁽ⁱ⁺¹⁾(γ_k)＝Γ(γ_k；η⁽ⁱ⁺¹⁾,θ⁽ⁱ⁺¹⁾) (20)

式中，η⁽ⁱ⁺¹⁾与θ⁽ⁱ⁺¹⁾分别表示变量γ_k伽马分布的形状与速率参数，分别通过下列公式计算：

式中，为偏差量的均值估计，/>为偏差量的估计协方差。

求解协方差矩阵Σ_k∣k-1的先验估计：

由下列公式给出：

式中，为中间参数。

求解参数τ_k的先验估计：

由下列公式给出：

步骤六：将步骤五中求解出的各个参数带入步骤四中进行计算，得到噪声的后验概率密度函数近似值；

根据式(20),(23),(26)，得出ξ_k,γ_k,Σ_k∣k-1,τ_k四个变量的期望计算公式：

式中，ψ(·)为双伽玛函数。

经过N次定点迭代，ξ_k的后验概率密度函数就被近似为：

其中，Σ_k∣k分别是不变误差和误差协方差的后验估计。

旋转状态R_k的分布改写为集中高斯分布，其均值为协方差为Σ_k∣k：

至此，计算出经过N次定点迭代后迭代更新得到姿态旋转矩阵的后验估计即可确定重尾噪声干扰环境下该时刻飞行器的旋转姿态矩阵。

本发明的有益效果：本发明建立分层高斯分布状态空间模型，提出了一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法。由于该技术手段基于历史观测数据为过程噪声预设概率密度函数，并通过贝叶斯迭代对模型的参数进行求解，解决了重尾噪声干扰下的飞行器姿态估计问题，可以显著提高飞行器李群位姿估计问题中IKF对重尾噪声滤波的鲁棒性。

附图说明

图1为本发明所述方法的流程框图。

图2为本发明所提出RSIKF滤波方法在不同N值下的ARMSE结果。

图3为α＝5,β＝10时各滤波方法的RMSE结果。

图4为α＝20,β＝10时各滤波方法的RMSE结果。

具体实施方式

以下结合附图对本发明作进一步详细的说明。这些附图均为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。

参照图1，一种基于信息反馈不变卡尔曼滤波器的自适应姿态估计方法，包括以下步骤：

S1.建立矩阵李群SO(3)上姿态估计的姿态动力学模型

SO(3)上的离散姿态动力学模型，如下所示：

R_k＝exp_G(w_k-1)R_k-1Ω_k-1 k＝1,2,... (1)

S2.将李群系统模型变换为李代数并转换到欧氏几何空间

不变误差ξ_k与模型参数Ω_k-1无关，即：

重新定义并线性化创新序列：

S3：基于观测数据定义重尾噪声中各参数的先验分布函数

设系统的过程噪声服从以下T分布：

尺度矩阵服从逆威希特分布：

自由度参数τ_k定义为伽马分布：

p(τ_k)＝Γ(τ_k；a_k,b_k) (7)

步骤四：后验概率密度函数的变分贝叶斯近似

对重尾噪声后验概率密度函数的准确解析表达式进行分解，获得其分解的近似值：

p(ξ_k,γ_k,Σ_k∣k-1τ_k∣z_1:k)≈q(ξ_k)q(γ_k)q(Σ_k∣k-1)q(τ_k) (8)

以上四个后验概率密度函数满足以下等式：

步骤五：定点迭代求系统状态与分布参数

基于ξ_k与γ_k,Σ_k∣k-1,τ_k三个参数的条件独立性，式(11)～(14)可以被改写为以下等式：

根据用定点迭代法分别求取ξ_k,γ_k,Σ_k∣k-1,τ_k四个参数。

求解不变误差ξ_k：

求解辅助随机变量γ_k：

q⁽ⁱ⁺¹⁾(γ_k)＝Γ(γ_k；η⁽ⁱ⁺¹⁾,θ⁽ⁱ⁺¹⁾) (18)

求解协方差矩阵Σ_k∣k-1的先验估计：

求解参数τ_k的先验估计：

步骤六：后验概率密度函数的变分贝叶斯近似计算

ξ_k,γ_k,Σ_k∣k-1,τ_k四个变量的期望计算公式：

经过N次定点迭代，ξ_k的后验概率密度函数就可以被近似为：

其中，Σ_k∣k分别是不变误差和误差协方差的后验估计。

那么，旋转状态R_k的分布可改写为集中高斯分布，其均值为协方差为Σ_k∣k：

实施例：

采用本发明提出的一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，给定系统参数和初始值为：

Σ_0|0＝0.5236²I_3×3,Σ_w＝0.01745²I_3×3,b'＝[1,0,0]^T,b”＝[0,1,0]^T,Σ_v'＝0.0873²I_3×3,Σ_v”＝0.0873²I_3×3.

滤波参数设置为λ_k＝k+4,a_k＝2,b_k＝6,N＝10.

真实姿态轨迹时长5000秒，重尾过程噪声与重尾异常值一起生成：

并且为了比较理论性能，使用5000次随机运行的李代数中的误差变量来计算5000s滤波后的均方根误差和平均均方根误差：

本例中，分别取α＝5,β＝10与α＝20,β＝10两种条件，对不同滤波方法在重尾噪声干扰下的性能进行模拟与比较，结果分别如图3、图4所示。

由上述仿真结果可以看出，相比于传统IKF、HKF等滤波方法，本发明所提出的RSIKF方法在以上实验条件下显著减小了滤波结果的均方差、平均均方差。因此，本发明所述的方法可以有效的解决重尾噪声干扰下的飞行器李群姿态估计问题，提高姿态估计的可靠性。

以上是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，凡是依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均属于发明技术方案的范围内。

Claims (5)

1.一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，其特征在于，包括以下步骤：

步骤一：建立矩阵李群SO(3)上姿态估计的离散姿态动力学模型，即李群动态模型；

步骤二：将系统的李群动态模型映射到欧氏几何空间；

步骤三：基于观测数据定义重尾噪声中各参数的先验分布函数；

步骤四：对重尾噪声的概率密度函数进行变分贝叶斯近似；所述的步骤四中，基于变分贝叶斯迭代，对重尾噪声后验概率密度函数的准确解析表达式进行分解，获得其分解的近似值：

p(ξ_k，γ_k，∑_k|k-1，τ_k，z_1：k)≈q(ξ_k)q(γ_k)q(∑_k|k-1)q(τ_k) (1)

其中，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)的值通过对因子近似式和准确解析表达式之间的相对熵进行最小化来确定：

{q(ξ_k)，q(γ_k)，q(∑_k|k-1)，q(τ_k)}

＝argminKLD(q(ξ_k)q(γ_k)q(∑_k|k-1)q(τ_k)||p(ξ_k，γ_k，∑_k|k-1，τ_k，z_1:k)) (2)

其中表示q(x)与p(x)之间的相对熵；根据变分贝叶斯迭代，以上近似后验q(ξ_k)，q(γ_k)，q(∑_k|k-1)，q(τ_k)满足以下等式：

其中，E_x[·]表示对除x以外的所有变量求数学期望；c_x是与x有关的常数；

步骤五：定点迭代求解系统状态与概率密度函数的分布参数；所述的步骤五中，基于ξ_k与γ_k，∑_k|k-1，τ_k三个参数的条件独立性，式(3)～(6)改写为以下等式：

其中，为转换后的测量方差矩阵，constant表示不影响后续迭代计算的常参数；

根据定点迭代法求解ξ_k，γ_k，∑_k|k-1，τ_k四个参数；

求解不变误差ξ_k：

式中，迭代后验估计与后验协方差/>通过以下等式计算：

式(10)中的表示第i+1次迭代的校准卡尔曼增益矩阵，/>为i+1次迭代的先验估计协方差；

求解辅助随机变量γ_k：

q⁽ⁱ⁺¹⁾(γ_k)＝Γ(γ_k；η⁽ⁱ⁺¹⁾，θ⁽ⁱ⁺¹⁾) (12)

式中，η⁽ⁱ⁺²⁾与θ⁽ⁱ⁺¹⁾分别表示变量γ_k伽马分布的形状与速率参数，分别通过下列公式计算：

式中，为偏差量的均值估计，/>为偏差量的估计协方差；

求解协方差矩阵∑_k|k-1的先验估计：

由下列公式给出：

式中，为中间参数；

求解参数τ_k的先验估计：

由下列公式给出：

步骤六：将步骤五中求解出的各个参数带入步骤四中进行计算，得到噪声的后验概率密度函数近似值。

2.根据权利要求1所述的一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，其特征在于，所述的步骤一中，SO(3)上姿态估计的离散姿态动力学模型如下所示：

R_k＝exp_G(w_k-1)R_k-1Ω_k-1 k＝1，2，... (21)

其中，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″表示。

3.根据权利要求2所述的一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，其特征在于，所述的步骤二中，设是R_k-1在k-1时刻的估计值，/>是R_k的估计值；定义不变误差/>ξ_k与模型参数Ω_k-1无关，其演化模型服从：

其中，O(·)为关于ξ_k-1和w_k的可忽略的二次以上小项；进一步忽略该二次项，则在李群动态模型下旋转矩阵R_k的求解就转化为在欧式空间中线性状态空间下对不变误差ξ_k的求解：

那么，观测序列Y_k则被重写为一个关于不变误差ξ_k的函数，新的序列表示为z_k，具有以下形式：

其中，和/>分别是映射后观测矩阵与噪声。

4.根据权利要求1所述的一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，其特征在于，所述的步骤三中，重尾噪声服从T分布：

其中，p(w_k)表示过程噪声w_k的概率密度；S_t(；μ，∑，τ)表示一个T分布，其均值为μ，尺度矩阵为∑，自由度为τ；γ_w是一个辅助随机变量；Γ(γ；α，β)代表一个标量γ的伽马分布概率密度函数，该标量分别以α和β作为形状与速率参数；为正态分布符号；

重尾噪声的尺度矩阵服从逆威希特分布，在姿态估计中，尺度矩阵的逆威希特概率密度函数写作：

其中IW表示逆威希特分布；λ_k表示自由度；Ψ_k是一个3×3的尺度矩阵的逆矩阵；|·|表示求行列式的操作；γ₃(·)是三变量的伽马分布；

重尾噪声的自由度参数τ_k为伽马分布：

p(τ_k)＝Γ(τ_k；a_k，b_k) (28)

其中，a_k，b_k分别表示τ_k伽马分布的形状与速率参数。

5.根据权利要求1所述的一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法，其特征在于，所述的步骤六中，根据式(12)，(15)，(18)，得出ξ_k，γ_k，∑_k|k-1，τ_k四个变量的期望计算公式：

式中，ψ(·)为双伽玛函数；

经过N次定点迭代，ξ_k的后验概率密度函数就被近似为：

其中，分别是不变误差和误差协方差的后验估计；

旋转状态R_k的分布改写为集中高斯分布，其均值为协方差为∑_k|k：

至此，计算出经过N次定点迭代后迭代更新得到姿态旋转矩阵的后验估计即可确定重尾噪声干扰环境下该时刻飞行器的旋转姿态矩阵。