CN111998854B - 基于Cholesky分解计算的精确扩展Stirling插值滤波方法 - Google Patents

Abstract

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

Description

基于Cholesky分解计算的精确扩展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时刻的系统状态变量的预测值

进行迭代更新，并计算每次迭代区间的中点方差矩阵；

步骤五、根据迭代后的系统状态变量的预测值

更新t_k时刻的观测值，并在系统状态变量的预测值

处计算t_k时刻的伪观测矩阵；

步骤六、根据伪观测矩阵及其对应的观测噪声方差矩阵将中点方差矩阵进行下三角矩阵变换，并根据下三角矩阵变换结果计算t_k时刻的系统状态变量的估计值和估计方差矩阵。

所述机器人SLAM系统连续-离散混合状态空间模型为：

其中，x_k表示t_k时刻的状态变量集合，x_k∈Rⁿ表示t_k时刻的状态变量，z_k∈R^m表示t_k时刻的观测向量，f(·)和h(·)均是非线性二阶可导函数，q(t)∈Rⁿ表示随时间变化的过程噪声，r_k∈R^m表示随时间变化的观测噪声，G(t)表示n×q的噪声方差矩阵，x(t)表示连续型系统状态变量；

因此，机器人SLAM系统的初始状态x₀属于一个已知集合x₀∈X₀，且系统初始状态满足统计特性

其中，

表示初始状态变量的估计值，Π₀表示系统初始状态方差矩阵，且

S₀为系统初始状态方差矩阵的平方根。

所述利用Stirling插值多项式对系统状态变量估计值

进行线性化的操作为：

其中，x_k表示t_k时刻的状态变量，f(·)是非线性二阶可导函数，D_△x项称为差分算子；

其中，

表示第k-1时刻的系统状态变量的估计偏差，μ_p为偏差算子，δ_p为平均算子；

所述偏差算子μ_p为：

所述平均算子δ_p为：

其中，

为沿轴向的单位向量，Δx_p表示系统状态变量解耦后的估计偏差量，h'为插值步长；

所述t_k时刻的系统状态变量的预测值

为：

其中，n表示系统状态变量维数；

预测方差矩阵P_k,k-1为：

其中，

所述在离散化观测采样区间内利用简化牛顿迭代法对t_k时刻的系统状态变量的预测值

进行迭代更新，并计算每次迭代区间的中点方差矩阵的方法为：

S41、设置采样时间间隔为δ，初始误差Δx₀＝0，在积分区间[t_k-1,t_k]中设置t₀＝t_k-1，取样时间τ₀＝min{10^-3,δ}，局部误差控制变量为

采样时间最大值设置为τ_max＝0.1，系统状态变量误差阈值为∈_g＝10^-4；

S42、在积分区间中当采样时刻t_l<t_k，且

表示积分间隔l中的系统状态变量偏差，执行采样时刻t_l+1＝t_l+τ_l，τ_l表示插值时间步长，利用简化牛顿迭代法的计算公式为：

令κ＝0,1,2,3,从而获得积分区间内的6点迭代计算公式为：

其中，

分别表示系统状态变量的计算节点，

分别表示系统函数在采样间隔的末尾节点值，

分别表示多点积分公式的系数值，τ_l表示采样间隔时间，

分别表示多点积分参数值，其在公式中的取值分别为，

I_n表示单位矩阵，

表示6点积分迭代值，

表示Jaccobian矩阵，

积分系数

S43、根据采样时刻t_l+1迭代计算获得

以及三次迭代值

和

则计算

时产生的局部误差为：

其中，le_l+1表示局部误差，若|le_l+1|_sc>∈_loc,则

表示比较确定出来的最小时间间隔，令

S44、在局部误差的基础上，全局误差估计为：

则全局误差绝对值估计为：

S44、计算每次迭代区间的中点方差矩阵：

其中，G_l+1/2＝G(t_l+1/2)表示中点过程噪声二阶矩，Q_l+1/2＝Q(t_l+1/2)表示中点过程噪声方差矩阵，

表示中点增益矩阵，

表示中点增益矩阵变换矩阵，Θ_l表示正交矩阵，S_l表示系统状态变量第l步迭代的估计方差矩阵平方根，满足

S45、确定第l+1步迭代的插值时间步长数值

S46、判断

若是，结束迭代，否则，更新局部误差控制变量

返回步骤S42执行下一步迭代。

所述根据迭代后的系统状态变量的预测值

更新t_k时刻的观测值的方法为：

S51、令

S_k,k-1＝S_L，t_k时刻的观测值为：

其中，

表示观测向量预测值，e_p为沿轴向的单位向量，s为插值步长；

S52、观测值对应的观测方差矩阵平方根为：

其中，

表示预测方差一阶平方根，

表示预测方差二阶平方根；

由预测方差矩阵和观测方差矩阵平方根可得预测协方差矩阵为：

所述t_k时刻的伪观测矩阵H_k为：

所述根据伪观测矩阵及其对应的观测噪声方差矩阵将中点方差矩阵进行下三角矩阵变换的方法为：

计算观测噪声方差：

其中，

表示观测噪声方差的平方根；

定义概念矩阵为：

其中，

表示预测增益矩阵，P_k,k-1表示预测方差矩阵，

表示伪观测矩阵H_k的转置，

表示预测方差平方根倒数；

利用正交矩阵Θ_k对中点方差矩阵进行下三角矩阵变换为：

其中，S_k为t_k时刻的估计方差矩阵的平方根，S_k,k-1表示预测方差矩阵的平方根。

所述根据下三角矩阵变换结果计算t_k时刻的系统状态变量的估计值为：

其中，

为t_k时刻的系统状态变量的估计值，

表示观测噪声预测方差平方根，y_k表示观测向量；

根据下三角矩阵变换结果计算t_k时刻的估计方差矩阵为：

其中，P_k为t_k时刻的估计方差矩阵，S_k为t_k时刻的估计方差矩阵的平方根。

本技术方案能产生的有益效果：

(1)本发明基于高阶Stirling插值多项式理论，提出了一种精确非线性扩展的Stirling插值滤波算法，二阶Stirling插值多项式计算相对简单，计算效率得到有效提高，能够满足组合导航系统初始对准的快速计算要求；

(2)Stirling插值多项式计算方法利用了固定步长面对非线性函数开展插值计算扩展逼近，有效避免了高阶微分计算过程，能够提供比较精确稳定的计算效率；

(3)针对传统的二阶Stirling插值多项式表达式开展离散观测化采样区间中的精确插值计算，实现插值计算的局部误差和全局误差的精确控制计算，从而有效改善非线性系统二阶Stirling插值多项式逼近计算稳定性和计算精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明的流程图。

图2是采用本发明方法计算获得的无人机位置计算数据图。

图3是采用本发明方法计算获得的无人机载体航向角计算数据图。

图4是EKF算法计算的无人机载体位置计算数据图。

图5是EKF算法计算的无人机载体航向角计算数据图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

1、预备知识

考虑一个随机微分方程为，

dX(t)＝F(X(t))dt+G(t)dW(t),t>0 (1)，

这里t时刻的n维系统状态向量X(t)∈Rⁿ，F(X(t))：Rⁿ→Rⁿ表示系统偏微分系数描述的系统动态函数，G(t)表示一个n×q的噪声方差矩阵，它随时间是可变的，{W(t)，t>0}表示系统噪声，具有零均值高斯白噪声特性，其方差为{Q(t)dt>0}_q×q，且其是正定性矩阵。系统状态变量初值设定为

其方差为Ω₀。

设计离散时间观测向量序列为{Z_k，k≥1}，它满足离散化观测方程为，

Z_k＝h(X_k)+V_k，k≥1 (2)，

观测方程中的k表示离散的t_k观测时刻，同样的X_k也表示时刻t_k的系统状态变量采样值，且{Z_k∈R^m,k≥1}表示采样时刻t_k的观测采样值，h(X_k)：Rⁿ→R^m表示离散化的观测函数，V_k表示观测噪声，其具有零均值高斯白噪声，噪声方差为R_k>0。

假设已知了t_k-1采样时刻的系统状态变量估计值，

及其方差矩阵为P_k-1，那么可以计算向量矩阵初始值问题为，

P(t_k-1)＝P_k-1

这里

表示Jaccobian矩阵，

那么在时间间隔[t_k-1,t_k]内预测计算系统状态变量及其预测方差矩阵，可分别表示为

和P_k,k-1＝P(t_k)，系统状态变量及其预测方差矩阵可由很多种方法积分计算出来，因此非线性微分方程的预测计算数据计算精度和计算稳定性完全由数值积分实施方法确定出来。那么本发明算法利用4阶或者6阶高斯型变步长NIRK数据对对其实施数值计算，且保持局部误差或者全局误差自动控制能力，其具体实施思路如下：

在数值积分区间[t_k-1,t_k]中指定数值积分网格，

对方程式(3)实施离散化计算为，

从而获得迭代递推计算公式为，

以上公式(5)和(6)中的网格步长为τ₁＝t_l+1-t_l，这里给出公式中的参数系数分别为，

上述的NIRK6阶插值计算公式(5)和(6)看起来很复杂，本发明算法实际运用过程中引入简化的牛顿迭代计算思想，其简化牛顿迭代计算公式为，

令κ＝0,1,2,3，从而获得迭代计算公式为，

方程解析式(9)是一个基于公式(3)在插值点

处的线性计算问题，I_n1表示单位矩阵，该式表明在一个网格插值点执行4次迭代步骤，那么

是时刻t_l+1的系统状态变量计算输出值，公式(9)的计算初始值设定为

对于方差矩阵Riccati方程式(4)，在系统状态变量每一次迭代计算后，展开对系统状态变量方差矩阵的Riccati迭代计算，其实施过程为，

这里t_l+1/2＝t_l+τ₁/2表示当前迭代步骤的中点时刻，其迭代中点时刻的可变矩阵计算公式为，

式中的

项表示在点

处的传统Jaccobian矩阵计算式，但是一般来说中点状态变量不可能得到的，那么在每一次迭代中，可采用

实现中点值替代计算。为了确保迭代计算过程中方差矩阵正定性或者半正定性，本发明算法引入方差矩阵平方根表示方法，对于初始方差矩阵可表示为，

并且假设过程噪声方差Q(t)和观测噪声方差R_k为对角矩阵，引入正交旋转矩阵Θ_l确保获得上三角矩阵，那么系统状态变量方差矩阵平方根可表示为，

在本发明算法实施中最复杂部分在于在采样时间区间[t_k-1,t_k]中执行自动网格划分问题，网格生成过程对算法计算精度产生重要作用，这里需要考虑非线性系统函数数值积分网格计算中的局部误差和全局误差控制问题，根据前面网格节点t_l+1迭代计算获得

以及三次迭代值

和

那么伴随

的局部误差可定义为，

但是局部误差le_l+1并不能确保实际计算误差趋于最小值，因此需要引入数值积分的全局误差概念，在局部误差定义基础上，全局部误差可定义为，

在实际计算中在采样时间区间[t_k-1,t_k]可设定全局误差初始值

在Taylor级数扩展滤波器算法的观测更新步骤中考虑

如图1所示，本发明实施例提供了一种基于Cholesky分解计算的精确扩展Stirling插值滤波方法，具体步骤如下：

步骤一、考虑机器人SLAM系统非线性系统函数方程和离散化观测方程形成连续-离散混合状态空间模型，根据随机连续微分方程和离散化观测方程构建机器人SLAM系统连续-离散混合状态空间模型，并设置机器人SLAM系统的状态变量初值；

所述机器人SLAM系统连续-离散混合状态空间模型为：

其中，X_k表示t_k时刻的状态变量集合，x_k∈Rⁿ表示t_k时刻的状态变量，z_k∈R^m表示t_k时刻的观测向量，f(·)和h(·)是已知的非线性二阶可导函数，q_k∈Rⁿ表示随时间变化的过程噪声，r_k∈R^m表示随时间变化的观测噪声，，并且满足统计特性条件，

记q_k∈(0,Q_k)和r_k∈(0,R_k)，G(t)表示n×q的噪声方差矩阵，x(t)表示连续型系统状态变量，q(t)表示连续型过程噪声向量；

因此，机器人SLAM系统的初始状态x₀属于一个已知集合x₀∈X₀，且系统初始状态满足统计特性

其中，

表示已知的系统状态变量初始值，Π₀表示系统初始状态方差矩阵，且

由系统状态的先验知识确定，S₀为系统初始状态方差矩阵的平方根，同时对于给定的测量序列向量

那么k时刻的Stirling插值滤波算法的状态变量集合定义为X_k，它由所有可能的状态点组成，这些状态点与所有可获取的信息，包括系统模型、噪声假设和初始状态集合相一致，并且设置系统状态变量误差阈值为∈_g＝10^-4。

步骤二、根据状态空间模型以及机器人SLAM系统的状态变量初值计算第t_k-1时刻的系统状态变量的估计值

和估计方差矩阵P_k-1，并对系统状态变量的估计方差矩阵进行J-正交Cholesky分解操作，得到

其中，S_k-1表示估计方差矩阵的平方根；假设连续-离散的机器人SLAM系统状态变量初始值及其统计特性已知，第t_k-1时刻(也表示为k-1)的状态变量估计及其统计特性经由步骤一得以计算；在非线性系统状态空间模型设计过程中，对于SLAM非线性系统函数实施线性化逼近计算，这里采用Stirling插值多项式开展非线性函数的线性化逼近计算，对于非线性系统函数在估计值

处实施Stirling插值多项式近似操作计算。

步骤三、利用Stirling插值多项式对系统状态变量估计值

进行线性化后预测t_k时刻的系统状态变量的预测值

和预测方差矩阵P_k,k-1；

所述利用Stirling插值多项式对系统状态变量估计值

进行线性化的操作为：

其中，x_k表示t_k时刻的状态变量，f(·)是非线性二阶可导函数，D_△x项称为差分算子；

其中，

表示第k-1时刻的系统状态变量估计偏差，μ_p为偏差算子，δ_p为平均算子；

所述偏差算子μ_p为：

所述平均算子δ_p为：

其中，

为沿轴向的单位向量，Δx_p表示系统状态变量解耦后的第k-1步骤的估计偏差，h'为插值步长；

所述t_k时刻的系统状态变量的预测值

为：

其中，n表示系统状态变量维数；

预测方差矩阵P_k,k-1为：

其中，

步骤四、在离散化观测采样区间内利用简化牛顿迭代法对t_k时刻的系统状态变量的预测值

进行迭代更新，并计算每次迭代区间的中点方差矩阵；具体方法为：

S41、设置采样时间间隔为δ，初始误差Δx₀＝0，在积分区间[t_k-1,t_k]中设置t₀＝t_k-1，取样时间τ₀＝min{10^-3，δ}，局部误差控制变量为

采样时间最大值设置为τ_max＝0.1，系统状态变量误差阈值为∈_g＝10^-4；

S42、在积分区间中当采样时刻t_l<t_k，且

表示时间间隔的系统状态变量差值，执行采样时刻t_l+1＝t_l+τ_l，τ_l表示插值时间步长，利用简化牛顿迭代法的计算公式为：

令κ＝0,1,2,3,从而获得积分区间内的6点迭代计算公式为：

其中，

均表示系统状态变量的计算节点，

分别表示系统函数在采样间隔的末尾节点值，

均表示多点积分公式的系数值，τ_l表示采样间隔时间，

均表示多点积分参数值。

S43、根据采样时刻t_l+1迭代计算获得

以及三次迭代值

和

则计算

时产生的局部误差为：

且

其中，le_l+1表示局部误差，若|le_l+i|_sc>∈_loc，则

表示比较确定出来的最小时间间隔值，令

S44、局部误差le_l+1并不能确保实际计算误差趋于最小值，因此需要引入数值积分的全局误差概念，在局部误差的基础上，全局误差估计为：

则全局误差绝对值估计为：

S44、计算每次迭代区间的中点方差矩阵：

其中，G_l+1/2＝G(t_l+1/2)表示中点过程噪声二阶矩，Q_l+1/2＝Q(t_l+1/2)表示中点过程噪声方差矩阵，

表示中点增益矩阵，

表示中点增益矩阵变换矩阵，Θ_l表示用来实现式(28)的右边下三角矩阵变换的正交矩阵，S_l表示系统状态变量第l步迭代的估计方差矩阵平方根，满足

S45、确定第l+1步迭代的插值时间步长数值

S46、判断

若是，结束迭代，否则，更新局部误差控制变量

返回步骤S42执行下一步迭代。

步骤五、根据迭代后的系统状态变量的预测值

更新t_k时刻的观测值，并在系统状态变量的预测值

处计算t_k时刻的伪观测矩阵；

所述根据迭代后的系统状态变量的预测值

更新t_k时刻的观测值的方法为：

S51、根据采样时刻

的计算结果，在设定系统状态变量误差阈值∈_g基础上，令

S_k,k-1＝S_L，t_k时刻的观测值为：

其中，

表示观测向量预测值，e_p为沿轴向的单位向量，h为插值步长；

S52、观测值对应的观测方差矩阵平方根为：

其中，

表示预测方差一阶平方根，

表示预测方差二阶平方根。

由预测方差矩阵和观测方差矩阵平方根可得预测协方差矩阵为：

在系统状态变量预测值

处计算t_k时刻的伪观测矩阵H_k为：

步骤六、根据伪观测矩阵及其对应的观测噪声方差矩阵将中点方差矩阵进行下三角矩阵变换，并根据下三角矩阵变换结果计算t_k时刻的系统状态变量的估计值和估计方差矩阵。

计算观测噪声方差：

其中，

表示观测噪声方差的平方根；

定义概念矩阵为：

其中，

表示预测增益矩阵，P_k,k-1表示预测方差矩阵，

表示伪观测矩阵H_k的转置，

表示预测方差平方根倒数。

利用正交矩阵Θ_k对进行下三角矩阵变换为：

其中，S_k为t_k时刻的估计方差矩阵的平方根，S_k,k-1表示预测方差矩阵的平方根。

所述根据下三角矩阵变换结果计算t_k时刻的系统状态变量的估计值为：

其中，

为t_k时刻的系统状态变量的估计值，

表示观测噪声预测方差平方根，y_k表示观测向量。

根据下三角矩阵变换结果计算t_k时刻的估计方差矩阵为：

其中，P_k为t_k时刻的估计方差矩阵，S_k为t_k时刻的估计方差矩阵的平方根。

具体实例：考虑机器人运动载体的SLAM问题，在笛卡尔坐标系下可以给出载体运动方程为：

这里SLAM系统的状态向量为x_k＝[x_k,y_k,φ_k]^T，分别表示第k步的载体位置坐标和方位；V是载体速度，G表示载体转向角，参数WB表示载体轮距，噪声向量v_k是高斯过程噪声，v_k～N(0,Q_k)，其中，Q_k表示噪声方差。

机器人运动载体配备了距离和方位传感器，能够在方位角±30°范围内感知距离30m范围内的目标物体，由此可以获得机器人SLAM系统的观测方程为：

其中，(r_i,x,r_i,y)是传感器感知的路标位置坐标，w_k是观测白噪声，满足分布w_k～N(0,R_k)，其中，R_k表示观测噪声方差。那么，SLAM系统的初始参数设置为：初始速度V₀＝3m/s，G＝±30°，WB＝4m，速度标准差σ_V＝0.3m/s，转向角标准差σ_G＝3°，距离标准差σ_r＝0.1m，方位标准差σ_B＝1°。初始状态向量x₀＝0，初始方差P₀＝diag{10^-10,10^-10,10^-10}。由此展开仿真验证工作，并且把精确扩展二阶Stirling插值滤波算法和传统扩展Stirling插值滤波算法计算效能对比，如图2、3、4、5所示。对比图2和图4，很明显两种算法中本发明方法与机器人真实运行轨迹数据拟合程度较好，而传统扩展Stirling插值滤波算法的拟合程度比较差些；从图3和图5可以对比看出，本发明的计算标准差比较小，误差数据曲线平滑稳定，而传统扩展Stirling插值滤波算法获得的位置误差数据变化剧烈，甚至出现数据发散现象，明显误差数据比较大，相应的标准偏差数据较大，通过利用这两种滤波器算法开展机器人SLAM系统的仿真实验，获得的实验数据说明，本发明的计算效能优于常规的传统扩展Stirling插值滤波算法，表现出了精确扩展二阶Stirling插值滤波算法的计算优势。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于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时刻的系统状态变量的预测值

进行迭代更新，并计算每次迭代区间的中点方差矩阵；

步骤五、根据迭代后的系统状态变量的预测值

更新t_k时刻的观测值，并在系统状态变量的预测值

处计算t_k时刻的伪观测矩阵；

步骤六、根据伪观测矩阵及其对应的观测噪声方差矩阵将中点方差矩阵进行下三角矩阵变换，并根据下三角矩阵变换结果计算t_k时刻的系统状态变量的估计值和估计方差矩阵。

2.根据权利要求1所述的基于Cholesky分解计算的精确扩展Stirling插值滤波方法，其特征在于，所述机器人SLAM系统连续-离散混合状态空间模型为：

其中，x_k表示t_k时刻的状态变量集合，x_k∈Rⁿ表示t_k时刻的状态变量，z_k∈R^m表示t_k时刻的观测向量，f(·)和h(·)均是非线性二阶可导函数，q(t)∈Rⁿ表示随时间变化的过程噪声，r_k∈R^m表示随时间变化的观测噪声，G(t)表示n×q的噪声方差矩阵，x(t)表示连续型系统状态变量；

因此，机器人SLAM系统的初始状态x₀属于一个已知集合x₀∈X₀，且系统初始状态满足统计特性

其中，

表示初始状态变量的估计值，Π₀表示系统初始状态方差矩阵，且

S₀为系统初始状态方差矩阵的平方根。

3.根据权利要求1所述的基于Cholesky分解计算的精确扩展Stirling插值滤波方法，其特征在于，所述利用Stirling插值多项式对系统状态变量估计值

进行线性化的操作为：

其中，x_k表示t_k时刻的状态变量，f(·)是非线性二阶可导函数，D_△x项称为差分算子；

其中，

表示第k-1时刻的系统状态变量的估计偏差，μ_p为偏差算子，δ_p为平均算子；

所述偏差算子μ_p为：

所述平均算子δ_p为：

其中，

为沿轴向的单位向量，Δx_p表示系统状态变量解耦后的估计偏差量，s为插值步长；

所述t_k时刻的系统状态变量的预测值

为：

其中，n表示系统状态变量维数；

预测方差矩阵P_k,k-1为：

其中，

4.根据权利要求3所述的基于Cholesky分解计算的精确扩展Stirling插值滤波方法，其特征在于，所述在离散化观测采样区间内利用简化牛顿迭代法对t_k时刻的系统状态变量的预测值

进行迭代更新，并计算每次迭代区间的中点方差矩阵的方法为：

S41、设置采样时间间隔为δ，初始误差Δx₀＝0，在积分区间[t_k-1,t_k]中设置t₀＝t_k-1，取样时间τ₀＝min{10^-3,δ}，局部误差控制变量阈值为

采样时间最大值设置为τ_max＝0.1，系统状态变量误差阈值为∈_g＝10^-4；

S42、在积分区间中当采样时刻t_l<t_k，且

表示积分间隔l中的系统状态变量偏差，执行采样时刻t_l+1＝t_l+τ_l，τ_l表示插值时间步长，利用简化牛顿迭代法的计算公式为：

令k＝0,1,2,3,从而获得积分区间内的6点迭代计算公式为：

其中，

分别表示系统状态变量的计算节点，

分别表示系统函数在采样间隔的末尾节点值，

分别表示多点积分公式的系数值，τ_l表示采样间隔时间，

分别表示多点积分参数值，其在公式中的取值分别为，

I_n表示单位矩阵，

表示6点积分迭代值，

表示Jaccobian矩阵，

积分系数

S43、根据采样时刻t_l+1迭代计算获得

以及三次迭代值

和

则计算

时产生的局部误差为：

其中，le_l+1表示局部误差，若|le_l+1|_sc>∈_loc，则

表示比较确定出来的最小时间间隔，令

S44、在局部误差的基础上，全局误差估计为：

则全局误差绝对值估计为：

S45、计算每次迭代区间的中点方差矩阵：

其中，G_l+1/2＝G(t_l+1/2)表示中点过程噪声二阶矩，Q_l+1/2＝Q(t_l+1/2)表示中点过程噪声方差矩阵，

表示中点增益矩阵，

表示中点增益矩阵变换矩阵，Θ_l表示正交矩阵，S_l表示系统状态变量第l步迭代的估计方差矩阵平方根，满足

S46、确定第l+1步迭代的插值时间步长数值

S47、判断

若是，结束迭代，否则，更新局部误差控制变量

返回步骤S42执行下一步迭代。

5.根据权利要求4所述的基于Cholesky分解计算的精确扩展Stirling插值滤波方法，其特征在于，所述根据迭代后的系统状态变量的预测值

更新t_k时刻的观测值的方法为：

S51、令

S_k,k-1＝S_L，t_k时刻的观测值为：

其中，

表示观测向量预测值，e_p为沿轴向的单位向量，s为插值步长，h(·)是非线性二阶可导函数；

S52、观测值对应的观测方差矩阵平方根为：

其中，

表示预测方差一阶平方根，

表示预测方差二阶平方根；

由预测方差矩阵和观测方差矩阵平方根可得预测协方差矩阵为：

6.根据权利要求5所述的基于Cholesky分解计算的精确扩展Stirling插值滤波方法，其特征在于，所述t_k时刻的伪观测矩阵H_k为：

7.根据权利要求5所述的基于Cholesky分解计算的精确扩展Stirling插值滤波方法，其特征在于，所述根据伪观测矩阵及其对应的观测噪声方差矩阵将中点方差矩阵进行下三角矩阵变换的方法为：

计算观测噪声方差：

其中，

表示观测噪声方差的平方根；

定义概念矩阵为：

其中，

表示预测增益矩阵，P_k,k-1表示预测方差矩阵，

表示伪观测矩阵H_k的转置，

表示预测方差平方根倒数；

利用正交矩阵Θ_k对中点方差矩阵进行下三角矩阵变换为：

其中，S_k为t_k时刻的估计方差矩阵的平方根，S_k,k-1表示预测方差矩阵的平方根。

8.根据权利要求5所述的基于Cholesky分解计算的精确扩展Stirling插值滤波方法，其特征在于，所述根据下三角矩阵变换结果计算t_k时刻的系统状态变量的估计值为：

其中，

为t_k时刻的系统状态变量的估计值，

表示观测噪声预测方差平方根倒数的转置矩阵，y_k表示观测向量，h(·)是非线性二阶可导函数；

根据下三角矩阵变换结果计算t_k时刻的估计方差矩阵为：

其中，P_k为t_k时刻的估计方差矩阵，S_k为t_k时刻的估计方差矩阵的平方根。

Images