一种新型的最大协熵椭球集员滤波方法与流程

本发明涉及自主移动机器人运动状态空间模型状态参数的滤波技术领域，特别是指一种新型的最大协熵椭球集员滤波方法。

背景技术：

滤波技术是研究如何从受各种干扰的信号观测结果中准确估计出未知的真实信号或者系统状态参数的一门技术。由于干扰和信号都具有随机的性质，这种估计技术只有采用统计学的方法才能解决。它依据一定的估计准则，按照某种统计方法实现对信号的准确估计计算。不同的估计准则，不同的观测序列和观测信号方式，会导致不同的估计方法。正因为如此，滤波理论技术经历了最小二乘理论、wiener滤波理论、卡尔曼滤波理论以及现代非线性滤波理论的发展而不断发展完善。但是在很多实际应用中，如自主移动机器人运动状态空间模型系统，所研究的动态系统一般都是非线性非高斯的，那么寻求一种能够从系统观测量中在线实时估计动态系统状态变量或系统参数的误差统计特性的有效方法，这是目前科学技术人员面临的一种重要挑战。

对于线性或者非线性系统滤波器算法，基于最小均方误差准则(minimummeansquareerrorcriterion,mmse)的经典kalman滤波器算法应用最为广泛，它基于面向线性或者非线性系统噪声的高斯假设条件下可以取得不错的估计效果，面向非线性状态空间模型系统的kalman滤波器算法称之为扩展kalman滤波器算法，但是在面向非高斯噪声应用中，kf或者ekf算法并不能获得最优滤波结果，因此研究新型的kf类滤波器算法成为kalman滤波理论算法发展的重要方向，如随后设计出来的无迹kalman滤波器算法(unscentedtransformationkf,ukf)、容积kalman滤波器算法(cubaturekalmanfilter,ckf)、中心差分滤波器算法(centerdifferentialkalmanfilter,cdkf)等，这些kf类算法都是基于mmse准则和高斯噪声假设条件下的逼近数值计算kalman滤波器算法，近年来基于信息学习理论中的熵准则被用于最优滤波计算中去，如本发明提出的最大协熵准则(maximumcorrentropycriterion,mcc)，它利用两个随机变量间的相似度统计矩阵函数作为代价函数设计一种最大协熵滤波器算法，特别对于异常数据或者脉冲噪声信号具有很强的鲁棒性特性，目前在机器学习、信号处理等领域获得较好的应用。

技术实现要素：

针对椭球集员滤波算法存在计算不稳定性的技术问题，本发明提出了一种新型的最大协熵椭球集员滤波方法，并将其应用到自主移动机器人位姿模型中开展机器人系统状态空间模型状态参数计算中去，提高了系统状态变量参数的计算稳定性与计算精度。

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

一种新型的最大协熵椭球集员滤波方法，其步骤如下：

步骤一、构建机器人非线性离散系统状态空间模型，并初始化非线性离散系统的系统状态变量，给出系统状态变量的椭球集合；

步骤二、根据系统状态变量的初始化结果获得k-1时刻的系统状态变量估计值和系统状态变量椭球集合；

步骤三、采用stirling插值多项式对第k-1时刻的系统状态变量估计值进行线性化逼近计算，并将线性化后的二阶多项式作为非线性离散系统的lagrange余子式；

步骤四、利用椭球将非线性离散系统的lagrange余子式外包得到线性化误差，并计算线性化误差的外包椭球；

步骤五、将线性化误差与非线性离散系统的过程噪声相加得到虚拟过程噪声误差，并计算虚拟过程噪声误差椭球，根据线性化误差和非线性离散系统的观测噪声计算虚拟观测噪声误差椭球；

步骤六、利用线性椭球集员滤波算法计算系统状态变量的线性化预测状态参数椭球边界、系统状态变量预测值和预测方差矩阵；

步骤七、根据虚拟过程噪声误差椭球和预测方差矩阵计算系统状态变量的增益矩阵；

步骤八、根据预测方差矩阵基于高斯核基的最大协熵准则构建机器人第k-1时刻的最大协熵代价函数，并将最大协熵代价函数的最大值作为优化目标函数；

步骤九、利用高斯牛顿方法对步骤八中的优化目标函数进行迭代计算，获得k时刻的状态变量的最优估计值和估计方差矩阵。

所述机器人非线性离散系统状态空间模型为：

其中，xk表示第k时刻的系统状态变量，xk-1表示第k-1时刻的系统状态变量，f(·)表示系统过程函数，h(·)表示观测方程函数，f(·)和h(·)都是已知的非线性二阶可导函数，qk-1∈rⁿ表示随时间变化的过程噪声，rk∈r^m表示随时间变化的观测噪声，yk表示观测向量；

所述系统状态变量的椭球集合为：

e(a,p)＝{x∈rⁿ|(x-a)^tp^-1(x-a)≤1}，

其中，a表示椭球集合的中心，p为正定性椭球包络矩阵，系统初始状态估计椭球集合为则k-1时刻估计得到的系统状态椭球集合为其中，p0为满足正定性的初始椭球包络矩阵，pk-1为满足正定性的k-1时刻的椭球包络矩阵，x0为系统的初始状态，e(·)表示椭球集合。

所述采用stirling插值多项式对第k-1时刻的系统状态变量估计值进行线性化逼近计算的方法为：

在确定性采样点χi处，采样点是由0、hei、-hei(1≤i≤n)、hei+hej(1≤i≤j≤n)组成，参数h是插值步长，按照概率高斯分布特点，则机器人非线性离散系统状态空间模型可表示为，

其中，si是积分点s∈r^m的第i个坐标轴单位向量，a∈r^m表示向量，h＝(hij)n×n为对称矩阵，n表示系统状态变量维数，表示实施状态变量解耦后的系统过程函数在第0积分点的函数映射；

所述非线性离散系统的lagrange余子式可表示为：

所述向量a∈r^m和对称矩阵h＝(hij)n×n的表达式为：

其中，ei表示沿第i轴向单位向量，ej表示沿第j轴向单位向量。

所述利用椭球将非线性离散系统的lagrange余子式外包得到线性化误差为：

其中，表示第k-1时刻由lagrange余子项确定的线性化误差形状矩阵的对角元，表示第k-1时刻由lagrange余子项确定的线性化误差形状矩阵的非对角元，i、j表示线性化误差形状矩阵的第i行第j列，取值为1≤i,j≤n，n为系统状态向量维数；

所述线性化误差的外包椭球为：其中，表示lagrange余子项确定的线性化误差形状矩阵。

所述虚拟过程噪声误差为：

其中，表示第k-1时刻的虚拟过程噪声，表示虚拟过程噪声方差矩阵，qk-1表示过程噪声矩阵，表示集合加操作，表示过程噪声误差优化因子；

所述虚拟过程噪声误差椭球为：其中，表示第k时刻的虚拟过程噪声；

所述虚拟观测噪声椭球为

所述线性化预测状态参数椭球边界为：

所述系统状态变量预测值为：

所述预测方差矩阵为：其中，是系统过程方程的一阶差分算子矩阵，βk-1表示预测方差矩阵尺度因子。

所述系统状态变量的增益矩阵为：

其中，表示虚拟观测噪声方差矩阵。

所述根据预测方差矩阵基于高斯核基的最大协熵准则构建机器人第k-1时刻的最大协熵代价函数为：

其中，jm(xk)表示高斯核基最大协熵函数，表示带宽数值为σ1的核函数，表示以观测方差为基础的观测向量分布函数，表示带宽数值为σ2的核函数，表示以预测方差为基础的系统状态变量分布函数，所述核函数和的表达式分别为：

其中，σ1≥0，σ2≥0均表示高斯核基带宽；

所述优化目标函数为：

其中，表示第k时刻的系统状态变量估计值。

所述利用高斯牛顿方法对步骤八中的优化目标函数进行迭代计算，获得k时刻的状态变量的最优估计值和估计方差矩阵的方法为：

s91、设置迭代次数t，将优化目标函数转化为高斯牛顿优化迭代表达式：

其中，当迭代次数t＝0时,k时刻的状态变量的估计初值k时刻的状态变量的预测误差方差矩阵初值表示梯度函数；

所述梯度函数的表达式为：

其中，表示伪观测矩阵，φ(xk)表示关于状态变量xk的代价函数的逼近hessian矩阵，φ(xk)的表达式为：

s92、令伪观测矩阵将梯度函数的表达式和φ(xk)的表达式代入高斯牛顿优化迭代表达式中，可得：

s93、利用前步迭代值对上式进行替换，可得：

s94、根据矩阵逆原理设置迭代增益矩阵则状态变量的迭代递推公式为：

s95、设置每一次迭代误差为根据步骤s54中的状态变量的迭代递推公式获得迭代误差表达式：

根据非线性观测方程式yk＝h(xk)+rk，利用对迭代误差表达式进行整理，可得，

根据整理后的迭代误差表达式计算估计误差方差矩阵：

s96、利用统计线性化方法更新伪观测矩阵：

其中，表示第t次迭代中的系统状态变量与观测向量的协方差矩阵，表示k时刻的状态变量的预测误差方差矩阵的逆矩阵；

利用第t次迭代计算的和计算中心差分滤波器的观测更新采样点及其权值系数，对方差矩阵执行cholesky分解操作，则第t次迭代的利用第t次迭代计算的和计算椭球集员滤波器的观测，对方差矩阵执行cholesky分解操作，根据第t次迭代的观测矩阵计算观测矩阵的方差矩阵：

其中，表示第t次迭代的观测矩阵的方差矩阵，rk表示表示第k时刻的观测方差矩阵；

计算观测噪声方差矩阵为：

计算观测更新中的观测变量的最大协熵值为：

s97、根据最大协熵值计算中心差分滤波器的增益矩阵：

计算第t次迭代的交叉协方差矩阵：

s98、迭代次数t＝t+1，判断迭代次数t是否达到最大迭代次数，若是，输出中心差分滤波器的增益矩阵和伪观测矩阵，执行步骤s59，否则，返回步骤s53；

s99、根据中心差分滤波器的增益矩阵和伪观测矩阵计算k时刻的状态变量的最优估计值和估计方差矩阵pk：

本技术方案能产生的有益效果：本发明基于mcc准则，构造非线性系统状态空间模型的最大协熵代价函数，设计非线性系统状态参数的确定性采样点及其权值系数，实现系统状态变量参数的时间更新计算，包括系统状态变量的预测值及其预测方差矩阵，进而利用观测方程实现系统状态变量及其观测误差矩阵的观测更新计算步骤，从而获得一种新型的最大协熵椭球集员滤波方法，将其应用到机器人运动状态空间模型状态参数滤波计算中，利用本发明方法开展陆基机器人位姿计算仿真验证，本发明的计算精度获得改善，计算稳定性相比于传统椭球集员滤波算法得到明显改善和提高。

附图说明

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

图1是本发明方法的计算流程图。

图2是本发明方法估计计算的机器人位姿误差数据图。

图3是本发明方法估计计算的机器人轨迹数据图。

图4是传统cdkf算法估计计算的机器人位姿误差数据图。

图5是传统cdkf算法估计计算的机器人轨迹数据图。

图6是传统ekf算法估计计算的机器人位姿误差数据图。

图7是传统ekf算法估计计算的机器人轨迹数据图。

具体实施方式

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

预备知识

协熵准则是一种局部相似度量概念，它定义核空间中两个随机向量间的相似度量，其优势在于对于较大误差的不敏感性，因此对于脉冲噪声或者异常数据处理不会对系统状态变量计算效能带来影响。针对两个随机变量x∈rⁿ,y∈rⁿ，其联合概率密度函数为px,y(x,y)，那么两个随机变量的协熵定义为，

协熵定义中的κ(x,y)是一个径向核函数，e[κ(x,y)]表示期望算子，径向核函数一般都是采用了高斯对称核函数表示的，

其中，x-y＝e表示两个变量向量的误差向量，σ表示核函数的带宽数值，σ＞0；但是对于非对称高斯核分布，其核函数可表示为，

这里定义的σ+和σ-分别表示误差向量的“+”的和“-”的部分，那么非对称协熵可定义为，

对于有限数据集合采用简单的采样均值估计器逼近连续型高斯核函数的协熵表达式为，

进一步获得taylor级数扩展表达式为，

若考虑状态空间模型的状态参数x和输出变量y，基于最大协熵准则开展迭代递推最优滤波计算获得系统状态变量最优估计目标值可表示为，

因此，本发明可以利用最大协熵准则构造代价函数开展非线性系统状态空间模型状态参数滤波算法设计工作。

本发明针对椭球集员滤波算法计算不稳定性，将其滤波计算准则从最小均方误差mmse修订为最大协熵准则mcc，从而设计获得一种新型的最大协熵椭球集员滤波算法，并将其应用到自主移动机器人位姿模型中开展机器人系统状态空间模型状态参数计算中去。本发明是在传统的基于最小均方误差准则的椭球集员滤波算法基础上，面向非线性系统状态空间模型，传统椭球集员滤波算法的时间更新预测步骤计算过程保留下来，在观测更新步骤中引入新型的最大协熵mcc准则，通过非线性系统预测噪声误差与观测噪声联合实施系统模型扩展操作来获得新的系统噪声表达式，根据基于系统状态变量预测向量与观测向量间的最大协熵准则构造二阶信息势能公式表达的误差代价函数，从而设计出椭球集员滤波算法的观测更新计算过程，由此构造出一种新型的基于最大协熵的椭球集员滤波算法计算框架。利用本发明方法开展陆基机器人位姿计算仿真验证，本发明的计算精度获得改善，计算稳定性相比于传统椭球集员滤波算法得到明显改善和提高。

最大协熵准则是一种基于renyis熵公式计算误差信息最大值的方法，引入机器学习理论中的信息势能概念，抽象出二阶信息势能表达式描述中心差分滤波器算法中的非线性系统状态变量的预测误差，连同非线性系统状态空间模型的观测向量估计误差，利用levenberg-marquardt优化方法计算最大协熵代价函数获得非线性系统状态变量的最优估计值。在第k-1时刻系统状态变量估计数据基础上，定义中心差分滤波器的系统状态变量的中心差分采样点及其权值系数，计算第k时刻系统状态变量预测值及其预测误差方差矩阵，在此基础上定义第k时刻的最大协熵代价函数表达式，由于观测函数的非线性性，这里引入非线性优化计算方法迭代逼近计算第k时刻系统状态变量的最优估计值。引入阻尼系数μ(μ≥0)设计levenberg-marquardt优化计算公式，以第k时刻预测值开展迭代优化计算，通过计算lm方法中的代价函数梯度和hessian矩阵，获得虚拟观测矩阵，从而获得系统状态变量的精确预测值及其预测方差矩阵；利用预测值开展中心差分观测更新计算，从而获得观测误差定义的最大协熵误差矩阵，系统状态变量观测更新增益矩阵，以及观测更新方差矩阵和协方差矩阵，从而获得系统状态变量的最优估计值数据。

如图1所示，本发明提出了一种新型的最大协熵椭球集员滤波方法，具体步骤如下：

步骤一、构建机器人非线性离散系统状态空间模型，并初始化非线性离散系统的系统状态变量，给出系统状态变量的椭球集合；

所述机器人非线性离散系统状态空间模型为：

其中，xk表示第k时刻的系统状态变量，xk-1表示第k-1时刻的系统状态变量，f(·)表示系统过程函数，h(·)表示观测方程函数，都是已知的非线性二阶可导函数，qk-1∈rⁿ和rk∈r^m表示随时间变化的过程噪声和观测噪声，并且满足未知但有界(ubb)假设条件，记qk∈(0,qk)和rk∈(0,rk)。系统的初始状态x0属于一个已知的有界集合x0∈x0，该集合可以由系统状态的先验知识确定，同时对于给定的测量序列向量那么k时刻的椭球集员滤波算法的状态可行集合定义为xk，它由所有可能的状态点组成，这些状态点与所有可获取的信息，包括系统模型、噪声假设和初始状态集合相一致。

定义椭球集合e(a,p)＝{x∈rⁿ|(x-a)^tp^-1(x-a)≤1}，其中a表示椭球集合的中心，p为满足正定性的椭球包络矩阵。定义系统初始状态估计椭球集合为那么k-1时刻估计得到的系统状态椭球集合为则k时刻非线性椭球集员滤波算法的迭代过程由步骤二至步骤九组成。

步骤二、根据系统状态变量的初始化结果获得k-1时刻的系统状态变量估计值和系统状态变量椭球集合

计算k-1时刻状态分量的不确定区间：

其中，i＝1,2,…,n,表示pk的(i,j)元素。

步骤三、采用stirling插值多项式对第k-1时刻的系统状态变量估计值进行线性化逼近计算，并将线性化后的二阶多项式作为非线性离散系统的lagrange余子式；k-1时刻的非线性系统状态空间模型的状态变量估计值为估计误差方差矩阵为pk-1，利用cholesky分解对k-1时刻非线性离散系统状态空间模型的估计误差方差矩阵进行分解操作，得到k-1时刻的估计误差方差矩阵的平方根：

其中，sk-1表示k-1时刻的估计误差方差矩阵的平方根，pk-1表示k-1时刻的估计误差方差矩阵；

k-1时刻的非线性系统状态空间模型的噪声方差估计误差矩阵qk-1进行cholesky分解操作，可得

在非线性系统状态空间模型设计过程中，对于运动目标的非线性系统函数实施线性化逼近计算，假设任意的非线性函数为，

y＝f(x)(11)，

对公式(11)实施gauss-hermite求积公式逼近计算为，

公式(12)实现利用2m-1阶多项式任意逼近非线性函数，中心差分滤波算法利用了二阶stirling插值多项式方法来实现，在确定性采样点χi处，采样点是由0、hei、-hei(1≤i≤n)、hei+hej(1≤i≤j≤n)组成，参数h是插值步长，按照概率高斯分布特点，则机器人非线性离散系统状态空间模型可表示为，

其中，si是积分点s∈r^m的第i个坐标轴单位向量，a∈r^m表示向量，h＝(hij)n×n为对称矩阵，n表示系统状态变量维数，表示实施状态变量解耦后的系统过程函数在第0积分点的函数映射；

所述向量a∈r^m和对称矩阵h＝(hij)n×n的表达式为：

其中，ei表示沿第i轴向单位向量，ej表示沿第j轴向单位向量。从而可以实现对非线性函数的数值积分逼近计算。

所述非线性离散系统的lagrange余子式可表示为：

步骤四、利用椭球将非线性离散系统的lagrange余子式外包得到线性化误差，并计算线性化误差的外包椭球；

线性化误差为：

其中，表示第k-1时刻由lagrange余子项确定的线性化误差形状矩阵的对角元，表示第k-1时刻由lagrange余子项确定的线性化误差形状矩阵的非对角元，i、j表示线性化误差形状矩阵的第i行第j列，取值为1≤i,j≤n，n为系统状态向量维数；

所述线性化误差的外包椭球为：其中，表示lagrange余子项确定的线性化误差形状矩阵。

步骤五、将线性化误差与非线性离散系统的过程噪声相加得到虚拟过程噪声误差，并计算虚拟过程噪声误差椭球，根据线性化误差和非线性离散系统的观测噪声计算虚拟观测噪声误差椭球；

虚拟过程噪声误差椭球

其中，表示第k-1时刻的虚拟过程噪声，是由线性化误差和过程噪声相加获得的，表示虚拟过程噪声方差矩阵，qk-1表示过程噪声矩阵，表示集合加操作，表示过程噪声误差优化因子；

对非线性观测方程开展类似计算步骤，得到虚拟观测噪声椭球

步骤六、利用线性椭球集员滤波算法计算系统状态变量的线性化预测状态参数椭球边界、系统状态变量预测值和预测方差矩阵；

利用线性椭球集员滤波算法的预测步骤计算预测状态参数椭球边界，这是线性化预测椭球和虚拟过程噪声直和计算过程

其中，是系统过程方程的一阶差分算子矩阵，βk-1表示预测方差矩阵尺度因子。

步骤七、根据虚拟过程噪声误差椭球和预测方差矩阵计算系统状态变量的增益矩阵；利用线性椭球集员滤波的更新步骤计算更新状态参数椭球边界其实质是将预测状态椭球和观测集合做直和交集计算。

其中，表示虚拟观测噪声方差矩阵。

步骤八、根据预测方差矩阵基于高斯核基的最大协熵准则构建机器人第k-1时刻的最大协熵代价函数，并将最大协熵代价函数的最大值作为优化目标函数；

所述第k-1时刻的最大协熵代价函数为：

其中，jm(xk)表示高斯核基最大协熵函数，表示带宽数值为σ1的核函数，表示以观测方差为基础的观测向量分布函数，表示带宽数值为σ2的核函数，表示以预测方差为基础的系统状态变量分布函数，所述核函数和的表达式分别为：

其中，σ1≥0，σ2≥0均表示高斯核基带宽；

在椭球集员滤波计算系统状态变量预测值和预测方差矩阵pk,k-1基础上，利用最大协熵代价函数获得当前步骤的估计值成为最大协熵中心差分滤波器算法设计的关键问题，因此设计优化目标函数为：

其中，表示第k时刻的系统状态变量估计值。

步骤九、利用高斯牛顿方法对步骤八中的优化目标函数进行迭代计算，获得k时刻的状态变量的最优估计值和估计方差矩阵。归因于非线性观测函数，式(10)的优化问题是无法获得解析解的，由此本发明算法引入了最优化迭代计算方法，可采用高斯牛顿方法实现式(10)的最优化迭代计算，具体方法为：

s91、设置迭代次数t，将优化目标函数转化为高斯牛顿优化迭代表达式：

其中，当迭代次数t＝0时,k时刻的状态变量的估计初值k时刻的状态变量的预测误差方差矩阵初值表示梯度函数；

所述梯度函数的表达式为：

其中，表示伪观测矩阵，φ(xk)表示关于状态变量xk的代价函数的逼近hessian矩阵，φ(xk)的表达式为：

s92、令伪观测矩阵将梯度函数的表达式和φ(xk)的表达式代入高斯牛顿优化迭代表达式中，可得：

s93、利用前步迭代值对上式进行替换，可得：

s94、根据矩阵逆原理设置迭代增益矩阵则状态变量的迭代递推公式为：

s95、设置每一次迭代误差为根据步骤s54中的状态变量的迭代递推公式获得迭代误差表达式：

根据非线性观测方程式yk＝h(xk)+rk，利用对迭代误差表达式进行整理，可得，

根据整理后的迭代误差表达式计算估计误差方差矩阵：

s96、利用统计线性化方法更新伪观测矩阵：

其中，表示第t次迭代中的系统状态变量与观测向量的协方差矩阵，表示k时刻的状态变量的预测误差方差矩阵的逆矩阵；

利用第t次迭代计算的和计算椭球集员滤波器的观测，对方差矩阵执行cholesky分解操作，根据第t次迭代的观测矩阵计算观测矩阵的方差矩阵：

其中，表示第t次迭代的观测矩阵的方差矩阵，rk表示第k时刻的观测方差矩阵；

计算观测噪声方差矩阵为：

计算观测更新中的观测变量的最大协熵值为：

s97、根据最大协熵值计算中心差分滤波器的增益矩阵：

计算第t次迭代的交叉协方差矩阵：

s98、迭代次数t＝t+1，判断迭代次数t是否达到最大迭代次数，若是，输出中心差分滤波器的增益矩阵和伪观测矩阵，执行步骤s59，否则，返回步骤s53；

s99、根据中心差分滤波器的增益矩阵和伪观测矩阵计算k时刻的状态变量的最优估计值和估计方差矩阵pk：

发明实例

把本发明方法应用于自主移动机器人场景中，可设计自主移动机器人运动状态变量xk＝[xk,yk,θk]^t，建立自主移动机器人运动模型为，

其中，t是采样周期，vk和wk分别是k时刻的机器人质心运动瞬时线速度和角速度，可由式计算获得，ε＝[εx,εy,εθ]^t是系统噪声项，a和b分别表示传感器与机器人质心之间的水平距离和垂直距离，假设机器人左右轮运动速度为输入量uk＝(vr,k,vl,k)^t。移动机器人利用安装在自身上的外部传感器获取环境地图的观测信息来构建地图，这里采用了距离方向传感器观测获取环境地图中的路标与传感器间的相对距离和夹角，观测值可用路标在传感器极坐标系下的坐标描述，表示为利用观测方程表示为，

其中，路标在全局坐标系中的位置坐标表示为假设第k时刻有n个路标，i＝1，2，····，n，μk表示观测噪声项；同时应用实例中假设路标位置是固定的，因此，环境地图动态模型可表示为由此开展本发明方法的仿真计算过程。同时考虑传统的cdkf和ekf算法作为对比，获得的仿真数据如图2-7所示。

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