[发明专利]多层感知和容积卡尔曼滤波的姿态估计融合方法及系统

权利要求书

1.一种基于多层感知和容积卡尔曼滤波的姿态估计多频数据融合方法，其特征在于：

建立视觉和惯性数据的滤波方程，采用CKF利用一组立方点来近似非线性函数的概率分布；

采用LWLR线性回归方法，利用高斯核函数对内存中测量点附近的点赋予较高的权值，求解最小均方差的无偏估计；

将视觉差异作为MLP的输入，并通过隐层的激活函数，将这种差异视作和的权重，用来补偿惯性数据，解决多频融合固有的散度。

2.根据权利要求1所述的基于多层感知和容积卡尔曼滤波的姿态估计多频数据融合方法，其特征在于：所述的建立视觉和惯性数据的滤波方程，

1)体积点计算

其中，[1]是主对角线为1的对称矩阵，下角标i表示第i列；ξ_i为体积点；ω_i为每个体积点所占的权值；n为系统维数；ω_i,k|k-1为k时刻预测体积点所占的权值；P_k-1|k-1为k-1时刻方差矩阵；为k-1时刻状态估计值；

2)时间更新

式(4)和式(5)分别定义了估计姿态和协方差矩阵分，其中k表示时间；

其中，是k时刻的状态估计值，X_i,k|k-1为体积点矩阵，P_k|k-1为一步预测协方差矩阵，T代表转置；

3)测量更新

体积点计算公式为(6)：

利用测量方程进行体积点传播，过程如下式所示：

Z_i,k|k-1＝h(X_i,k|k-1) (7)

其中，Z_i,k|k-1为k时刻观测估计矩阵，为k时刻观测估计值，h为非线性量测函数；

一步预测测度互协方差矩阵P_xz,k|k-1和一步预测测度协方差矩阵P_zz,k|k-1定义如下，其中k为时间，T为转置运算：

卡尔曼滤波增益K_k计算如下：

姿态估计量和姿态误差的协方差P_k|k定义如下：

其中，z_k为k时刻观测值，是k时刻的状态估计值,T为转置运算。

3.根据权利要求1或2所述的基于多层感知和容积卡尔曼滤波的姿态估计多频数据融合方法，其特征在于：所述采用LWLR线性回归方法，利用高斯核函数对测量点附近的点赋予较高的权值，权值表达式如下所示：

其中ω为测点附近点的权值，i代表第i个测量点，x表示待测点，k是控制权重变化率的参数，为避免k值较小时的过拟合和k值较大时的欠拟合，LWLR算法将k设定为1。

4.根据权利要求2所述的基于多层感知和容积卡尔曼滤波的姿态估计多频数据融合方法，其特征在于：隐层的激活函数采用sigmoid函数，其定义如下：

5.一种基于多层感知和容积卡尔曼滤波的姿态估计多频数据融合系统，包括存储器和处理器，存储器存储有计算机程序，其特征在于；所述处理器执行所述计算机程序时实现如权利要求1-4任一所述的方法步骤。