[发明专利]适于INS/CNS组合导航系统的基于最大相关熵的广义高阶CKF算法

权利要求书

1.一种适用于INS/CNS组合导航系统的基于最大相关熵的广义高阶CKF算法，其特征在于:该方法包括以下步骤：

(1)构造INS/CNS组合导航系统滤波模型；

(2)根据构造的滤波模型进行滤波算法的时间更新；

(3)引入最大相关熵准则和判断准则，进行滤波算法的量测更新。

2.根据权利要求1所述的一种适用于INS/CNS组合导航系统的基于最大相关熵的广义高阶CKF算法，其特征在于:步骤(1)所述的构造INS/CNS组合导航系统滤波模型包括以下步骤：

(11)设置INS/CNS组合导航系统的状态向量为x＝[φ,δv,δr,ε,Δ]，其中[φ,δv,δr,ε,Δ]分别表示INS姿态误差、速度误差、位置误差、陀螺常值漂移和加速度计常值偏置；

(12)根据INS/CNS组合导航系统的状态向量x建立系统的状态方程：

其中，F表示状态转移矩阵，G表示过程噪声输入矩阵，W表示过程噪声；对上述方程进行离散化，得到离散化的系统状态方程：

x_k＝f(x_k-1,W_k-1)

其中，f(·)为已知的非线性函数，x_k-1和x_k分别表示k-1时刻和k时刻的状态向量，W_k-1表示k-1时刻的过程噪声并且其均值为0，W_k-1的协方差可表示为：

取导航系统的数学平台误差角方程作为系统的量测方程，

z_k＝H_kx_k+V_k

其中，z_k表示k时刻的量测向量，H_k表示k时刻的测量矩阵，V_k表示k时刻的量测噪声其满足均值为0，它的协方差表示为：并且V_k与W_k-1满足互不相关。

3.根据权利要求1所述的一种适用于INS/CNS组合导航系统的基于最大相关熵的广义高阶CKF算法，其特征在于:步骤(2)包括以下步骤

(21)根据INS/CNS组合导航系统滤波精度要求设置核宽度λ和阈值c的值，设置初始的状态向量的估计值为初始的误差协方差为P_0|0,k＝1,对P_0|0进行乔列斯分解得到误差协方差阵的特征平方根初值S_0|0；

(22)根据公式计算出容积点X_i,k-1|k-1(i＝1,...,2n²+1)，其中n表示状态向量的维数，[·]_i表示集合[·]的第i列，例如时，有[1]＝{[1 0]^T,[0 1]^T,[-1 0]^T,[0 -1]^T}；

(23)计算传播容积点

(24)预测当前时刻的状态向量误差协方差阵P_k|k-1和计算P_k|k-1的特征平方根S_k|k-1，其中

4.根据权利要求3所述的一种适用于INS/CNS组合导航系统的基于最大相关熵的广义高阶CKF算法，其特征在于：引入最大相关熵准则和判断准则，进行滤波算法的量测更新，

(31)根据预测的状态向量和P_k|k-1的特征平方根S_k|k-1产生新的容积点X_i,k|k-1和传播容积点Z_i,k|k-1，Z_i,k|k-1＝H_kX_i,k|k-1，其中i＝1,...,2n²+1；

(32)预测k时刻量测值

(33)根据INS/CNS组合导航系统的量测方程和步骤(24)中状态向量和P_k|k-1的计算公式，构建如下回归方程,

其中，I表示单位向量，

M_p,k|k-1、M_r,k和M_k通过的乔列斯分解得到；

在回归方程的两边同时乘以得到：D_k＝B_kX_k+e_k；其中，

(34)对量测噪声协方差阵进行更新得到其中，diag(·)表示矩阵的对角化，m表示量测向量的维数，相关熵中的核函数d_i,k表示D_k的第i个元素，b_i,k表示B_k的第i行元素；

(35)计算量测向量的协方差P_zz,k|k-1，

(36)计算如果|a_k|＞c，则P_k|k＝P_k|k-1，k＝k+1，返回步骤(22)继续执行下一滤波周期，如果|a_k|≤c，则计算k＝k+1，返回步骤(22)继续执行下一滤波周期。