一种最小误差熵CDKF滤波器方法与流程

本发明涉及机器人导航定位技术领域，特别是指一种最小误差熵cdkf滤波器方法。

背景技术：

传统的中心差分滤波方法是bayesian最优滤波理论框架下利用stiring插值逼近数值计算方法获得的一类次优滤波计算算法，它和传统的kalman滤波算法、扩展kalman滤波算法、无迹kalman滤波算法和容积kalman滤波算法一样都是采用不同的数值逼近计算方法获得非线性系统状态空间模型状态变量参数的最优或者次优的迭代滤波计算方法，都属于概率分布意义上的点逼近最优估计值的处理算法。它们具有共同的特点就是基于高斯噪声分布特点基础上采用了流行的最小均方误差准则(minimummeansquareerror，mmse)开展迭代滤波计算过程，但是mmse准则在复杂噪声场景应用并不是一个很好的选择，尤其是对于非高斯噪声场景，上述算法存在计算性能退化问题。

近年来针对重尾或者脉冲非高斯噪声问题，一些非mmse准则被应用到鲁棒kalman滤波算法中来避免滤波器算法性能退化问题，其中信息学习论中的最大协熵准则(maximumcorrentropycriterion，mcc)被应用到kalman滤波算法设计中来应对脉冲噪声干扰，从而构造出了最大协熵kalman滤波算法、最大协熵扩展kalman算法、最大协熵无迹kalman算法以及最大协熵平方根容积kalman算法等，其优势在于最大协熵准则是一种局部相似度量准则，其对较大的误差不敏感，因此经由最大协熵设计的kalman类算法的计算性能不会受到比较大的外界数据影响。在信息学习理论中还有一种最小误差熵准则(minimumerrorentropy，mee)，也在鲁棒收敛分析、分类学习、系统辨识和自适应滤波算法中获得重要应用，并且经由数值分析验证，mee准则在很多应用方面的计算性能远高于mcc准则，如在计算复杂度方面，mee准则明显比mcc准则的计算复杂度低。

技术实现要素：

针对传统cdkf算法计算不稳定的技术问题，本发明提出了一种最小误差熵cdkf滤波器方法，利用最小误差熵准则，基于二阶信息势能设计二阶最小误差熵的中心差分滤波算法，利用一步预测误差和观测误差设计扩展误差模型系统，计算扩展误差系统状态方差及其平方根方差矩阵，实现误差噪声的模型变换计算，构造二阶最小误差熵信息势能计算表达公式，对其展开偏微分计算获取系统状态变量的最优估计值，进而计算其最优估计方差矩阵。

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

一种最小误差熵cdkf滤波器方法，其步骤如下：

步骤一、构建陆基机器人的非线性离散系统状态空间模型，利用二阶stirling插值多项式对非线性离散系统进行数值积分逼近计算，得到非线性离散系统的确定性采样点及线性表达式；

步骤二、根据k-1时刻非线性离散系统状态空间模型的状态变量估计值估计误差方差矩阵pk-1，获得k-1时刻的状态变量估计值的确定性采样点和加权系数；

步骤三、根据k-1时刻的状态变量估计值的确定性采样点确定加权采样点集合，并预测k时刻的非线性离散系统的状态变量预测值

步骤四、根据非线性离散系统的状态变量预测值获取非线性离散系统的状态变量的预测误差，并根据预测误差对非线性离散系统状态空间模型进行扩展整理获得非线性离散系统的扩展噪声项；

步骤五、根据非线性离散系统的扩展噪声项计算扩展噪声误差，根据renyis熵基于二阶信息势能构建扩展噪声误差的最小误差熵代价函数，通过最小化最小误差熵代价函数获得非线性离散系统的状态变量最优值

步骤六、计算最小误差熵代价函数的偏微分方程，根据偏微分方程利用逆矩阵计算原理获得状态变量最优值的估计值估计方差矩阵和估计协方差矩阵；

步骤七、根据步骤六获得的状态变量最优值的估计值设置参数τ，令判断若是，输出状态变量最优值的估计值估计方差矩阵和估计协方差矩阵，执行步骤八，否则，返回步骤六，其中，表示第k时刻系统状态变量上一步迭代估计值；

步骤八、根据状态变量最优值的估计方差矩阵计算非线性离散系统的状态变量的后验方差矩阵。

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

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

所述利用二阶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轴向单位向量。

所述k-1时刻的状态变量估计值的确定性采样点和加权系数的获得方法为：

利用cholesky分解对k-1时刻非线性离散系统状态空间模型的估计误差方差矩阵进行分解操作，得到k-1时刻的估计误差方差矩阵的平方根：

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

利用二阶stirling插值多项式对k-1时刻非线性离散系统状态空间模型的状态变量估计值和估计误差方差矩阵的平方根进行逼近操作，获得k-1时刻的状态变量估计值的确定性采样点：

其中，χ0,k-1表示根据第k-1时刻的系统状态变量估计值确定的中心采样点，χi,k-1表示根据第k-1时刻的系统状态变量估计值确定的除中心采样点外的其余采样点，表示k-1时刻非线性离散系统状态空间模型的状态变量估计值；

根据二阶stirling插值多项式的插值步长h确定k-1时刻的状态变量估计值的确定性采样点的加权系数：

其中，表示确定性中心采样点的加权均值系数，表示第i个确定性采样点的加权均值系数，表示第i个确定性采样点的加权协方差系数。

所述k时刻的非线性离散系统的状态变量预测值为：

其中，χi,k,k-1＝f(χi,k-1)表示第k时刻的第i个采样点的加权预测值。

所述非线性离散系统的状态变量的预测误差为：

其中，表示系统状态变量的预测误差；

所述根据预测误差对非线性离散系统状态空间模型进行扩展整理获得非线性离散系统的扩展噪声项的方法为：

对非线性离散系统状态空间模型进行扩展可得，

其中，in表示n维单位矩阵，hk表示观测函数的一阶jaccobian矩阵；

定义扩展噪声项为

所述根据非线性离散系统的扩展噪声项计算扩展噪声误差的方法为：

计算扩展噪声项μk的方差矩阵为，

其中，θk表示的cholesky分解算子矩阵，θp,k,k-1表示pk,k-1的cholesky分解算子矩阵，θr,k表示rk的cholesky分解算子矩阵；

将非线性离散系统状态空间模型的扩展模型的两边乘以整理获得，

dk＝wkxk+ek，

其中，并且dk＝(d1,k,d2,k,…,dl,k)^t，wk＝(w1,k,w2,k,…,wl,k)^t，ek＝(e1,k,e2,k,…,el,k)^t为扩展噪声误差，且l＝n+m。

所述根据renyis熵基于二阶信息势能构建扩展噪声误差的最小误差熵代价函数为：

其中，gσ表示高斯核基函数，jl(xk)表示最小误差熵代价函数，i1＝1,2,…,l，j1＝1,2,…,l；

非线性离散系统的状态变量最优值为：

所述最小误差熵代价函数的偏微分方程为：

其中，

所述状态变量最优值的估计值为：

其中，λy,k表示根据第k时刻的观测向量获得的最小代价函数转换矩阵；

估计方差矩阵为：其中，表示状态变量最优值的估计方差矩阵，λx,k表示根据第k时刻的状态向量获得的最小代价函数转换矩阵；

估计协方差矩阵为：其中，和表示状态变量与观测向量间转换后的协方差矩阵，λyx,k和λxy,k表示状态变量与观测向量间转换矩阵。

所述非线性离散系统的状态变量的后验方差矩阵为：

其中，pk表示状态变量的后验方差矩阵。

本技术方案能产生的有益效果：本发明通过观测更新步骤的二阶最小误差熵信息势能微分计算，有效改善了传统cdkf算法的计算不稳定性问题，经由陆基机器人运动状态仿真验证，mee-cdkf算法的计算效能获得改善，计算精度得到保证。

附图说明

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

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

图2是本发明方法的移动机器人运动模型示意图。

图3是本发明方法mee-cdkf的移动机器人载体计算误差数据图。

图4是本发明方法mee-cdkf的移动机器人载体轨迹计算图。

图5是ekf算法获得的移动机器人载体计算误差数据图。

图6是ekf算法获得的移动机器人载体轨迹计算图。

图7是srukf算法获得的移动机器人载体计算误差数据图。

图8是srukf算法获得的移动机器人载体轨迹计算图。

具体实施方式

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

预备知识

所谓的最小误差熵，不同于传统的mmse或者mcc准则，其目标是最小化包含误差的信息，假设误差信息为e，其定义采用了renyis熵概念，

其中，α表示renyis熵的阶次，要求α＞0,α≠1，vα(e)表示信息势能，其定义为，

vα(e)＝∫p^α(x)dx＝e[p^α-1(e)](2)

其中，p(e)表示误差e的概率密度函数，e[p^α-1(e)]表示期望算子，在实际中心差分滤波算法设计中概率密度函数可由parzen窗估计计算出来，

这里gσ(x-ei)是高斯核基函数为，是n个误差采样数据序列。若取renyi熵的阶次α＝2，那么二阶信息势能v2(e)为，

由于负对数函数是单调递减的，最小化误差熵h2(e)，这个式子意味着就是最大化信息势能

根据中心差分滤波算法计算框架，若cdkf算法状态预测更新中的预测误差定义为，

其中表示状态预测值，若非线性系统状态空间模型方程表达式为，

对其实施taylor级数展开逼近计算，在处对过程函数实施线性化逼近计算，在处对观测函数实施线性化逼近计算，获得，

这里，对式(5)进行变换，结合式(7)，在此基础上对系统模型进行扩展，可以获得，

并对式(8)中的扩展噪声项定义为它融合了状态预测误差和观测误差信息，那么其方差矩阵可计算为，

这里的θk、θp,k,k-1和θr,k分别表示pk,k-1和rk的cholesky分解算子矩阵，将前面式子(8)两边都乘以整理获得，

dk＝wkxk+ek(10)

其中并且dk＝(d1,k,d2,k,…,dl,k)^t，wk＝(w1,k,w2,k,…,wl,k)^t，ek＝(e1,k,e2,k,…,el,k)^t，且l＝n+m。

基于二阶信息势能式(4)，定义代价函数，

那么可以利用最小化代价函数式(11)获得非线性系统状态变量估计值

从而计算代价函数对xk的偏导数，

这里(ψk)ij＝gσ(ei,k-ej,k)，由此可利用固定点迭代计算系统状态变量估计，

这里且满足λk∈r^l×l，λx,k∈r^n×n，λxy,k∈r^m×n，λyx,k∈r^n×m，λy,k∈r^m×m，从而可以获得，

从而对以上两式(15)和(16)进行整理规范为，

根据式(15)和(16)，这里矩阵π1π2π3的定义为，

根据矩阵逆计算定理，将式(17)整理获得系统状态变量估计迭代递推计算式为，

这里

相应的系统状态变量后验方差矩阵可计算为，

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

最小误差熵准则是一种基于renyis熵公式计算误差信息最小值的方法，引入机器学习理论中的信息势能概念，抽象出二阶信息势能表达式描述cdkf算法中的非线性系统状态变量的预测误差，连同非线性系统状态空间模型的观测方程，获得非线性系统扩展误差状态模型，计算扩展误差状态模型的噪声方差，并对其进行cholesky分解获得扩展噪声的平方根方差正定矩阵，利用平方根噪声方差矩阵对扩展误差状态模型进行变换整理获得预测误差与观测噪声联合误差表达式，进而利用最小误差熵准则的二阶信息势能表达式综合出cdkf预测误差代价函数，那么利用最小误差熵代价函数最小化来获得系统状态变量的最优化计算，具体做法就是对最小化误差熵代价函数计算偏微分并令其为0来获得，同时利用矩阵逆定理实现系统状态变量最优化计算显性表达式，并且在计算机算法编制中设置一个小量参数对每一次迭代计算的系统状态变量估计值进行判断，若判断表达式成立，则继续进行系统状态变量估计方差矩阵计算；否则的话继续进行前面步骤的计算过程，最后获得系统状态变量估计方差矩阵的计算。

如图1所示，本发明实施例提供了一种最小误差熵cdkf滤波器方法，具体步骤如下：

步骤一、构建陆基机器人的非线性离散系统状态空间模型，利用二阶stirling插值多项式对非线性离散系统进行数值积分逼近计算，得到非线性离散系统的确定性采样点及线性表达式；所述非线性离散系统状态空间模型为：

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

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

y＝f(x)(23)，

实施gauss-hermite求积公式逼近计算为，

它可以实现利用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轴向单位向量；从而可以实现对非线性函数的数值积分逼近计算。

步骤二、根据k-1时刻非线性离散系统状态空间模型的状态变量估计值估计误差方差矩阵pk-1，获得k-1时刻的状态变量估计值的确定性采样点和加权系数；

利用cholesky分解对k-1时刻非线性离散系统状态空间模型的估计误差方差矩阵进行分解操作，得到k-1时刻的估计误差方差矩阵的平方根：

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

利用二阶stirling插值多项式对k-1时刻非线性离散系统状态空间模型的状态变量估计值和估计误差方差矩阵的平方根进行逼近操作，获得k-1时刻的状态变量估计值的确定性采样点：

其中，χ0,k-1表示根据第k-1时刻的系统状态变量估计值确定的中心采样点，χi,k-1表示根据第k-1时刻的系统状态变量估计值确定的除中心采样点外的其余采样点，表示k-1时刻非线性离散系统状态空间模型的状态变量估计值；

根据二阶stirling插值多项式的插值步长h确定k-1时刻的状态变量估计值的确定性采样点的加权系数：

其中，表示确定性中心采样点的加权均值系数，表示第i个确定性采样点的加权均值系数，表示第i个确定性采样点的加权协方差系数。

步骤三、根据k-1时刻的状态变量估计值的确定性采样点确定加权采样点集合，并预测k时刻的非线性离散系统的状态变量预测值

所述k时刻的非线性离散系统的状态变量预测值为：

其中，χi,k,k-1＝f(χi,k-1)表示第k时刻的第i个采样点的加权预测值。

步骤四、根据非线性离散系统的状态变量预测值获取非线性离散系统的状态变量的预测误差，并根据预测误差对非线性离散系统状态空间模型进行扩展整理获得非线性离散系统的扩展噪声项；

所述非线性离散系统的状态变量的预测误差为：

其中，表示状态变量预测误差；

所述根据预测误差对非线性离散系统状态空间模型进行扩展整理获得非线性离散系统的扩展噪声项的方法为：

对非线性离散系统状态空间模型进行扩展可得，

其中，in表示n维单位矩阵，hk表示观测函数的一阶jaccobian矩阵；

定义扩展噪声项为扩展噪声项融合了状态预测误差和观测误差信息。

步骤五、根据非线性离散系统的扩展噪声项计算扩展噪声误差，根据renyis熵基于二阶信息势能构建扩展噪声误差的最小误差熵代价函数，通过最小化最小误差熵代价函数获得非线性离散系统的状态变量最优值

计算扩展噪声项μk的方差矩阵为，

其中，θk表示的cholesky分解算子矩阵，θp,k,k-1表示pk,k-1的cholesky分解算子矩阵，θr,k表示rk的cholesky分解算子矩阵；

将非线性离散系统状态空间模型的扩展模型的两边乘以整理获得，

dk＝wkxk+ek(34)，

其中，并且dk＝(d1,k,d2,k,…,dl,k)^t，wk＝(w1,k,w2,k,…,wl,k)^t，ek＝(e1,k,e2,k,…,el,k)^t为扩展噪声误差，且l＝n+m。

所述根据renyis熵基于二阶信息势能构建扩展噪声误差的最小误差熵代价函数为：

其中，gσ表示高斯核基函数，jl(xk)表示最小误差熵代价函数，i1＝1,2,…,l，j1＝1,2,…,l；

非线性离散系统的状态变量最优值为：

步骤六、计算最小误差熵代价函数的偏微分方程，根据偏微分方程利用逆矩阵计算原理获得状态变量最优值的估计值估计方差矩阵和估计协方差矩阵；

所述最小误差熵代价函数的偏微分方程为：

其中，

根据偏微分方程，利用固定点迭代计算系统状态变量估计，

其中，且满足λk∈r^l×l，λx,k∈r^n×n，λxy,k∈r^m×n，λyx,k∈r^n×m，λy,k∈r^m×m，从而可以获得，

从而对以上两式(39)和(40)进行整理规范为，

根据式(39)和(40)，这里矩阵π1π2π3的定义为，

利用矩阵逆计算定理，将式(41)整理获得状态变量最优值的估计值为：

其中，λy,k表示根据第k时刻的观测向量获得的最小代价函数转换矩阵；

估计方差矩阵为：其中，表示状态变量最优值的估计方差矩阵，λx,k表示根据第k时刻的状态向量获得的最小代价函数转换矩阵；

估计协方差矩阵为：其中，和表示状态变量与观测向量间转换后的协方差矩阵，λyx,k和λxy,k表示状态变量与观测向量间转换矩阵。

步骤七、根据步骤六获得的状态变量最优值的估计值设置参数τ，令判断若是，输出状态变量最优值的估计值估计方差矩阵和估计协方差矩阵，执行步骤八，否则，返回步骤六，其中，表示第k时刻系统状态变量上一步迭代估计值；

步骤八、根据状态变量最优值的估计方差矩阵计算非线性离散系统的状态变量的后验方差矩阵。

所述非线性离散系统的状态变量的后验方差矩阵为：

其中，pk表示状态变量的后验方差矩阵。

应用实例

为了验证本发明提出的最小误差熵中心差分滤波算法的计算效能，利用本发明方法对陆基机器人定位系统模型开展仿真验证计算，来证明本发明方法的有效性及其计算优势，这里给出仿真验证测试数据。考虑一个地面移动机器人系统，它采用前轮驱动模式，如图2所示，定义前轮转向角为α，以逆时针方向为正方向，机器人坐标系xyrobot相对于地面坐标系xyground的转向角度为ψ，以逆时针方向为正，后轮速度定义为vrearwheel。从几何图形可以看出观测点到后轮中心的距离r和前后轮轴距l之间满足

从而可以获得观测点距离，同时我们可以得到后轮速度表达式为可以整理获得机器人转向角方程为，

整理获得

若再考虑机动机器人系统的运动速度，可以在机器人坐标系中得到机动机器人系统的动力学方程为

把机器人系统动力学方程转换到地面坐标系中，可以获得最终的地面坐标系中的移动机器人系统移动方程

系统方程中的参数α(t)可作为调制参数，作为系统的输入变量，产生一个前轮的控制参数，其满足控制律

这里的参数ψdes表示期望的航向角，参数ψ表示当前的运动航向角，那么期望得到的航向角可以表达为

一般来说转向角的范围在(±π/4)内，g参数影响着机器人转向的速率快慢。对于移动机器人系统运动方程目的是开展系统状态变量估计和运动路径跟踪，因此有很多传感器来感知系统坐标位置，如gps或者正向编码测向仪等设备来完成目标跟踪观测，本发明选择在地面坐标系中的移动机器人的两向位置坐标作为观测变量，因此观测方程是线性的，可以直接获得的。考虑系统初始方差矩阵为

另外系统参数设置为移动机器人前后轮轴距l＝2m，控制律增益g＝2，移动速度保持为v＝1m/s，采样时间间隔δt＝0.1s；假设过程变量中仅有航向角ψ有干扰噪声，可设过程噪声方差为q＝0.05²。观测变量是移动机器人位置坐标，因此观测噪声方差矩阵可设为

从而可以获得移动机器人系统的移动轨迹仿真结果，这里采用了ekf算法和srukf算法和本发明算法mee-cdkf算法计算结果进行对比，其结果如图3-图8所示。利用ekf算法和srukf算法对本发明mcc-cdkf算法进行比较，可以看到，mee-cdkf算法的计算稳定性比较好，且其计算收敛速度快，计算精度获得明显改善与提高。

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