一种最大协熵扩展椭球集员滤波方法与流程

文档序号:23068445发布日期:2020-11-25 17:56阅读:来源:国知局

技术特征:

1.一种最大协熵扩展椭球集员滤波方法,其特征在于,其步骤如下:

步骤一、构建陆基自主移动机器人的非线性离散系统状态空间模型,并初始化非线性离散系统的系统状态变量,给出系统状态变量的椭球集合;其中,非线性离散系统状态空间模型包括动力学运动模型方程和离散化观测模型方程;

步骤二、根据非线性离散系统状态空间模型的初始化结果获得第k-1时刻的系统状态变量估计值和系统状态变量椭球形状矩阵pk-1,并对系统状态变量椭球形状矩阵pk-1进行cholesky分解操作;

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

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

步骤五、将线性化误差与非线性离散系统的过程噪声相加得到虚拟过程噪声误差,并计算虚拟过程噪声误差椭球,对虚拟过程噪声误差椭球进行实施cholesky分解操作;

步骤六、利用线性椭球集员滤波算法计算系统状态变量的状态参数椭球边界和系统状态变量预测值并根据虚拟过程噪声误差椭球预测系统状态变量的椭球形状矩阵;

步骤七、基于离散化观测模型方程和第k-1时刻的系统状态变量估计值对非线性离散系统的状态变量进行预测更新获得观测向量的预测值和预测椭球形状矩阵,并根据预测椭球形状矩阵的平方根和步骤六中的椭球形状矩阵的平方根计算预测协方差矩阵;

步骤八、根据步骤六中的系统状态变量预测值计算状态变量预测误差,并将步骤六中的系统状态变量预测值和观测方程相结合获得系统误差扩展方程;利用系统误差扩展方程的扩展噪声项构造的方差矩阵,根据方差矩阵获得转换扩展状态模型;

步骤九、根据步骤八的转换扩展状态模型,利用mcc准则构造代价函数,通过计算代价函数的最优解获得第k时刻的系统状态变量的最优估计值

步骤十、利用线性椭球集员滤波算法更新系统状态变量的状态参数椭球边界,根据状态参数椭球边界计算第k时刻的系统状态向量的预测值和预测方差矩阵。

2.根据权利要求1所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述非线性离散系统状态空间模型为:

其中,xk∈rn表示k时刻的状态变量,xk-1表示k-1时刻的状态变量,f(·)和h(·)均表示非线性二阶可导函数,qk-1表示第k-1时刻的过程噪声项,rk∈rm表示随时间变化的观测噪声项,yk表示观测向量;

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

e(a,p)={x∈rn|(x-a)tp-1(x-a)≤1},

其中,a表示椭球集合的中心,p为正定性椭球包络矩阵,系统初始状态估计椭球集合为

3.根据权利要求2所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述采用stirling插值多项式对第k-1时刻的系统状态变量估计值进行线性化逼近计算的表达式为:

其中,xk表示k时刻的状态变量,d△x项称为差分算子;

其中,△xp表示系统状态变量解耦后的估计偏差量,μp为偏差算子,δp为平均算子,表示第k-1时刻的系统状态变量的估计偏差,s为插值步长;

所述偏差算子μp为:

所述平均算子δp为:

其中,为沿轴向的单位向量;

所述lagrange余子式为:

其中,表示以第k-1时刻的状态估计值为中心的lagrange余子项。

4.根据权利要求3所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述利用椭球将非线性离散系统的lagrange余子式外包得到线性化误差为:

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

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

5.根据权利要求4所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述虚拟过程噪声误差为:

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

所述虚拟过程噪声误差椭球为:其中,表示第k时刻的虚拟过程噪声;

对虚拟过程噪声误差椭球进行实施cholesky分解操作,得到:其中,表示虚拟过程噪声方差矩阵平方根。

6.根据权利要求5所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述系统状态变量预测值为:

其中为沿轴向的单位向量;

所述系统状态变量的椭球形状矩阵为:

其中,表示系统状态变量预测形状矩阵,表示p′k,k-1的平方根;

7.根据权利要求6所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述观测向量的预测值

其中,表示系统观测向量预测值;

预测协方差矩阵pxy,k,k-1为:

其中,表示系统观测方程预测误差矩阵;

8.根据权利要求7所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述状态变量预测误差为:

其中,表示状态变量预测误差;

所述系统误差扩展方程为:

其中,表示非线性观测方程的一阶差分算子矩阵,bk表示第k时刻的一阶差分算子矩阵,rk表示观测噪声项,i表示单位矩阵,是扩展噪声项;

所述利用系统误差扩展方程的扩展噪声项构造的方差矩阵为:

其中,θk表示扩展噪声方差阵,sk表示扩展噪声方差阵θk的cholesky分解矩阵,sr,k表示观测噪声误差矩阵的平方根,表示扩展噪声平方根矩阵;

所述转换扩展状态模型为:

dk=wkxk+ek,

其中,表示第k时刻的扩展状态,表示第k时刻的扩展状态转移矩阵,

9.根据权利要求8所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述代价函数为:

其中,di,k是矩阵dk的第i行向量,wi,k是矩阵wk的第i行向量,l=n+m表示矩阵dk的维数,jl(xk)表示mcc代价函数,gσ表示误差代价函数;

所述第k时刻的系统状态变量的最优估计值为:

通过对代价函数的求偏导及整理可得:

其中,cx,k=diag(gσ(d1,k-w1,kxk),…,gσ(dn,k-wn,kxk)),

cy,k=diag(gσ(dn+1,k-wn+1,kxk),…,gσ(dn+m,k-wn+m,kxk))。

10.根据权利要求9所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述利用线性椭球集员滤波算法更新系统状态变量的状态参数椭球边界为:其中,pk表示第k时刻的系统状态变量估计误差矩阵,ek表示更新的系统状态变量椭球;

所述根据状态参数椭球边界计算第k时刻的系统状态向量的预测值为:

其中,表示椭球集员滤波器增益矩阵;

所述预测方差矩阵:

其中,是观测方程的一阶差分算子矩阵。


技术总结
本发明提出了一种最大协熵扩展椭球集员滤波方法,属于机器人领域的导航定位授时服务中的系统信息处理技术,具体涉及利用导航定位传感数据构建机器人运动载体非线性状态空间模型。本发明基于最小均方误差准则的椭球集员滤波算法,引入新型的最大协熵MCC准则,通过非线性系统预测噪声误差与观测噪声联合实施系统模型扩展操作来获得新的系统噪声表达式,基于系统状态变量预测向量与观测向量间的最大协熵准则构造误差代价函数,从而设计出椭球集员滤波算法的观测更新计算过程,由此构造出一种新型的基于最大协熵的椭球集员滤波算法计算框架;达到了有效改善传统集员滤波算法的计算稳定性问题,从而完成机器人载体组合导航定位服务功能。

技术研发人员:丁国强;赵朋朋;凌丹;赵素娜;刘娜;娄泰山;张焕龙;王晓雷;王妍
受保护的技术使用者:郑州轻工业大学
技术研发日:2020.08.31
技术公布日:2020.11.24
当前第2页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1