加权卡尔曼滤波融合,它通过给不同传感器或估计结果分配不同的权重,来提高状态估计的精度和可靠性。
一、卡尔曼滤波
1.状态方程
2.观测方程
其中:
基本方程
①状态一步预测
②状态估计
③滤波增益
④一步预测均方差
⑤估计均方误差
二、加权卡尔曼滤波
对于多个传感器,分别构成的卡尔曼滤波器,进行加权:
对于加权系数的求解
当P最小时(即),所得到的误差最小,即为最优估计;
运用拉格朗日构造法,令:
求导;即:
根据上式得
其中
得到
三、滤波过程
①对n个传感器分别建立卡尔曼滤波器
②求得对应得估计及其方差,以及相互间得协方差
③计算加权系数
其中,
④计算加权最优估计
参考文献:
[1]
X. Ding, Z. Wang, L. Zhang and C. Wang, "Longitudinal Vehicle Speed Estimation for Four-Wheel-Independently-Actuated Electric Vehicles Based on Multi-Sensor Fusion," in IEEE Transactions on Vehicular Technology, vol. 69, no. 11, pp. 12797-12806, Nov. 2020, doi: 10.1109/TVT.2020.3026106. keywords: {Estimation;Sensors;Tires;Wheels;Kalman filters;Vehicle dynamics;Roads;Multi-sensor fusion;longitudinal vehicle speed estimation;road gradient estimation;Kalman filter;four-wheel-independently-actuated electric vehicles},