[发明专利]一种平方根单形容积卡尔曼滤波方法

说明书

技术领域

本发明属于非线性滤波领域，具体涉及一种平方根单形容积卡尔曼滤波方法。

背景技术

非线性滤波是现代控制的研究热点之一，广泛存在于目标跟踪、卫星轨道/姿态估计、探测器的导航与控制等工程应用问题当中。贝叶斯滤波是一种通过适当的假设和近似，可以递归解算的方法，是解决非线性滤波问题的主流思想，也得到了广泛的研究和应用，并取得了一系列成果。

其中，扩展卡尔曼滤波(EKF)算法是贝叶斯滤波方法的传统算法，它通过泰勒展开式对非线性函数进行一阶线性化近似，忽略了系统的非线性特性，在初始化误差较大或系统非线性较强时滤波精度变差甚至发散。

Julier提出的无迹卡尔曼滤波(UKF)算法利用一组选定的sigma点进行UT变换，从而逼近非线性函数的概率分布，相对EKF算法，可以更好地捕获非线性变换的高阶矩。然而，UKF算法仍然存在滤波精度不足，数值稳定性差的问题。

近年来提出了一种基于三阶球面-径向容积法则的容积卡尔曼滤波(CKF)算法，已经得到了广泛的工程应用。相对于UKF而言，CKF有着更高的数值稳定性。但是，CKF算法受限于三阶容积法则，不能获取更高的滤波精度。

为此，Bin Jia提出了一种高阶的容积卡尔曼滤波(HCKF)算法，它能够以任意阶次精度逼近非线性函数的概率分布，提高了三阶CKF的滤波精度。然而，HCKF算法在选用较高阶次进行非线性逼近时，计算量会大幅增加，同时滤波过程中仍然存在数值稳定性较差的问题。

为了减少HCKF的计算量，Shiyuan Wang基于球面单形-径向法则，提出了一种球面单形容积卡尔曼滤波(SSRCKF)算法。SSRCKF算法在系统状态维度大于4时，计算量少于HCKF算法，同时有着更高的滤波精度。但是在滤波递推过程中存在种种数值计算的舍入误差，不能确保数值稳定性。

发明内容

为了克服现有技术的不足，本发明提供一种平方根单形容积卡尔曼滤波(SR-SSRCKF)方法。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1、给定系统初始状态均值和误差协方差矩阵P_0|0＝P(0)，然后计算单形采样的容积基点ξ_i及其相应的权值ω_i；