1.一种最大协熵扩展椭球集员滤波方法,其特征在于,其步骤如下:
步骤一、构建陆基自主移动机器人的非线性离散系统状态空间模型,并初始化非线性离散系统的系统状态变量,给出系统状态变量的椭球集合;其中,非线性离散系统状态空间模型包括动力学运动模型方程和离散化观测模型方程;
步骤二、根据非线性离散系统状态空间模型的初始化结果获得第k-1时刻的系统状态变量估计值
步骤三、采用stirling插值多项式对第k-1时刻的系统状态变量估计值
步骤四、利用椭球将非线性离散系统的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为平均算子,
所述偏差算子μp为:
所述平均算子δp为:
其中,
所述lagrange余子式为:
其中,
4.根据权利要求3所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述利用椭球将非线性离散系统的lagrange余子式外包得到线性化误差为:
其中,
所述线性化误差的外包椭球为:
5.根据权利要求4所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述虚拟过程噪声误差为:
其中,
所述虚拟过程噪声误差椭球为:
对虚拟过程噪声误差椭球进行实施cholesky分解操作,得到:
6.根据权利要求5所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述系统状态变量预测值
其中
所述系统状态变量的椭球形状矩阵为:
其中,
7.根据权利要求6所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述观测向量的预测值
其中,
预测协方差矩阵pxy,k,k-1为:
其中,
8.根据权利要求7所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述状态变量预测误差为:
其中,
所述系统误差扩展方程为:
其中,
所述利用系统误差扩展方程的扩展噪声项构造的方差矩阵为:
其中,θk表示扩展噪声方差阵,sk表示扩展噪声方差阵θk的cholesky分解矩阵,sr,k表示观测噪声误差矩阵的平方根,
所述转换扩展状态模型为:
dk=wkxk+ek,
其中,
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所述的最大协熵扩展椭球集员滤波方法,其特征在于,所述利用线性椭球集员滤波算法更新系统状态变量的状态参数椭球边界为:
所述根据状态参数椭球边界计算第k时刻的系统状态向量的预测值为:
其中,
所述预测方差矩阵:
其中,