本技术提出的一种基于超半球均匀确定性采样的四元数粒子滤波姿态估计方法,属于数据处理技术领域,主要用于解决粒子滤波器在姿态估计中存在的实时性较差、计算量较大的问题,同时可以避免粒子枯竭与退化对姿态估计算法带来的影响。该方法以四元数粒子滤波为框架,嵌入超半球均匀确定性采样,通过在半球均匀采样的样本镜像到整个球体,进而通过指数映射的方式得到四元数样本,以确定性样本代替传统随机样本,根据量测值和权值计算公式更新权值,在保证滤波精度的同时具有更低的计算成本,适用于非线性系统的姿态估计、传感器数据融合等应用场合。
背景技术
非线性滤波问题出现在许多领域,其中包括目标跟踪、捷联惯导系统以及飞行器姿态估计等。然而,对于飞行器姿态估计这样的强非线性系统,精确且稳定的序列姿态估计并不容易。为了减小陀螺仪不可避免的漂移所带来的偏差,实现长期稳定的估计,传感器数据融合算法是不可缺少的。IMU姿态估计定义在称为SO(3)的特殊正交旋转群中,其非线性结构是四元数滤波面临的主要问题。使用四元数作为姿态参数不仅避免了欧拉角的万向节死锁问题,并且在只有一个程度冗余的情况下表示了SO(3)的双重覆盖结构。四元数估计方法分可分为两种,分别是间接估计法和直接估计法。间接估计使用姿态误差参数作为状态向量,依赖于假设噪声为高斯分布的局部非线性线性化,如扩展卡尔曼滤波,无迹四元数滤波,但是受限于局部参数化和线性化继承的基本问题;直接估计方法使用单位四元数作为状态向量,单位四元数构成了包括的双覆盖的旋转群的良好表示,并且同时只有一个冗余,可以在保证精度的前提下提高数据处理性能。
直接方法比间接方法在姿态估计精度和收敛速度方面有更好的性能,但通常需要更大的计算量,采用确定性采样的方案可以很好的改善这一缺点。因此,对超半球面上的确定性样本使用狄拉克混合来实现以非参数形式模拟单位四元数流形的不确定性,同时由于单位四元数的反足对称性,进一步地在超半球上获得任意数量的确定性样本,并通过镜像和指数映射投影到单位四元数流形可以进一步减小计算量。但是目前,在四元数粒子滤波中,研究基于超半球采样的确定采样方式的相关内容还较少。
实现思路