[发明专利]一种最大协熵MCC准则的椭球集员滤波方法

权利要求书

1.一种最大协熵MCC准则的椭球集员滤波方法，其特征在于，其步骤如下：

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

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

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

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

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

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

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

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

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

其中，J_m(x_k)表示高斯核基的最大协熵函数，x_k表示第k时刻的系统状态变量，x_k-1表示第k-1时刻的系统状态变量，f(•)表示系统过程函数，h(•)表示观测方程函数，y_k表示观测向量，表示带宽数值为σ₁的核函数，表示以观测方差为基础的观测向量分布函数，表示带宽数值为σ₂的核函数，表示以预测方差为基础的系统状态变量分布函数，所述核函数和的表达式分别为：

其中，σ₁≥0，σ₂≥0均表示高斯核基带宽，R_k表示第k时刻的观测方差矩阵，P_k,k-1表示预测方差矩阵；

所述优化目标函数为：

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

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

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

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

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

其中，表示伪观测矩阵，Φ(x_k)表示关于系统状态变量x_k的代价函数的逼近Hessian矩阵，Φ(x_k)的表达式为：

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

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

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

其中，表示迭代次数t+1时第k时刻的状态变量的估计值，表示系统状态变量预测值，表示迭代增益矩阵，表示伪观测矩阵，表示迭代次数t时第k时刻的状态变量的估计值，h(·)表示观测方程函数，y_k表示观测向量；

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

根据非线性观测方程式y_k＝h(x_k)+r_k，r_k表示随时间变化的观测噪声，利用对迭代误差表达式进行整理，可得，

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

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

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

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

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

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

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

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

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

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