[发明专利]基于Cholesky分解计算的精确扩展Stirling插值滤波方法

说明书

本发明提出了一种基于Cholesky分解计算的精确扩展Stirling插值滤波方法，用于实现SLAM系统状态空间模型状态参数最优滤波计算，属于导航定位与控制领域。本发明基于SLAM状态空间模型非线性动态方程与离散化观测数据，利用Stirling插值多项式逼近计算获得SLAM系统等价模型方程，根据离散化观测数据的采样区间确定非线性系统方程Stirling插值多项式的精确积分计算；针对传统NIRK积分开展局部和全局误差控制计算，将其数值积分计算过程融入到二阶Stirling插值多项式一阶均值和二阶方差逼近计算中来实现新型二阶Stirling滤波算法设计过程。经由SLAM系统仿真，并与传统二阶扩展Stirling插值滤波算法对比，验证本发明算法的计算优势和计算效能。

技术领域

本发明涉及机器人系统技术领域的导航定位授时(Positioning、Naviagtion andTiming,PNT)服务中的机器人系统信息处理技术领域，具体涉及一种基于Cholesky分解计算的精确扩展Stirling插值滤波方法。

背景技术

滤波技术是研究如何从受干扰的信号观测结果中准确估计出未知的真实信号或者系统状态变量参数的一门技术，它依据一定的估计准则，按照某种估计方法实现对信号的准确滤波计算，不同的估计准则、不同的观测序列数据、和观测信号方式会导致估计方法的差异，因此滤波技术经历了最小二乘理论、Wiener滤波理论、Kalman滤波理论以及现代的非线性滤波理论算法发展而不断完善与提高。传统Bayesian随机概率滤波理论要求已知过程噪声和观测噪声的统计特性，或者假设其满足一定的概率分布条件，而实际的非线性系统中系统状态或者参数的统计特性往往是非高斯、非线性的，因此常规的非线性随机概率滤波算法的应用有很大的局限性。如扩展Kalman滤波算法的高阶截断误差影响，中心差分滤波算法的高阶项截尾影响等，那么寻求一种从系统观测量中在线实时最优估计动态系统状态变量参数的最优滤波计算是科技人员面临的重大挑战。

对于非线性状态空间模型中的非线性系统方程和观测方程式，基于Taylor级数线性化处理得到的扩展集员滤波算法存在着很大的缺陷，首先当系统非线性比较强时，围绕系统状态参数预测估计或者状态参数预估值的一阶Taylor级数展开式往往存在着很大的截断误差，使得该算法存在数值计算稳定性变差，计算复杂，甚至出现滤波算法发散现象；再者一阶Taylor级数扩展需要计算Jacobi矩阵，二阶Taylor级数扩展需要计算复杂的Hessian矩阵，计算量巨大，对处理器要求很高，难以满足实际系统快速计算要求。

发明内容

针对上述背景技术中存在的不足，本发明提出了一种基于Cholesky分解计算的精确扩展Stirling插值滤波方法，解决了Taylor级数扩展表达式计算的复杂度高，扩展Kalman滤波算法的计算精度差的技术问题。

本发明的技术方案是这样实现的：

一种基于Cholesky分解计算的精确扩展Stirling插值滤波方法，其步骤如下：

步骤一、构建机器人SLAM系统连续-离散混合状态空间模型，并设置机器人SLAM系统的状态变量初值特性数据；

步骤二、根据状态空间模型以及机器人SLAM系统的状态变量初值计算第t_k-1时刻的系统状态变量的估计值和估计方差矩阵P_k-1，并对系统状态变量的估计方差矩阵进行J-正交Cholesky分解操作，得到其中，S_k-1表示估计方差矩阵的平方根；

步骤三、利用Stirling插值多项式对系统状态变量估计值进行线性化后预测t_k时刻的系统状态变量的预测值和预测方差矩阵P_k,k-1；

步骤四、在离散化观测采样区间内利用简化牛顿迭代法对t_k时刻的系统状态变量的预测值进行迭代更新，并计算每次迭代区间的中点方差矩阵；