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

权利要求书

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为：

其中，