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

说明书

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

技术领域

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

背景技术

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

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

发明内容

针对重尾噪声干扰下的不变卡尔曼滤波姿态估计，本发明提出一种分层高斯状态空间模型：基于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无关，其演化模型服从：