首页 > 编程知识 正文

卡尔曼滤波推导,扩展卡尔曼滤波与卡尔曼滤波

时间:2023-05-03 14:37:36 阅读:171079 作者:4849

对于松耦合融入状态预测过程,最典型的是卡尔曼滤波,其主要作用是对多个传感器进行融合处理,估计出良好的状态值。 处理一般分为松散结合和紧密结合。

松耦合是指只将传感器的结果数据作为观测值,更新预测值。

例如,将IMU作为预测的原始数据进行计算,将GPS定位的结果作为观测值进行误差修正更新,在逻辑上简单实用,而且通过使用多传感器进行融合可以提高精度。

另一方面,在严密结合的情况下,使用GPS的伪距数据和其他更原始、更完整的数据进行计算很复杂。

例如,

在进行EKF预测运算时,使用IMU作为预测数据进行计算,一旦产生IMU数据,立即进行姿势预测,将GPS数据和LiDAR数据等其他传感器的数据作为参照观测值使用,在产生观测值数据时,使用观测值修正预测值

因此,松散耦合使用的传感器越多越容易修正估计误差,以IMU为预测数据传感器,其他传感器为观测传感器是主流。

有关卡尔曼滤波的详细介绍,建议在IMU中使用博文深度分析卡尔曼滤波。 写得很详细。

在此,简单提取主要步骤以供参考。

两个模型的状态方程

xk=FX k1 buk kx _ k=FX _ { k-1 } bu _ kw _ kxk=FX k1 buk wk

测量方程

z k=H x k v k z_k=Hx_k v_k zk=Hxk vk

预测阶段k时刻的系统状态先验估计

x^kk1=fx^k1k1

+ B u k hat {x}_{k|k-1} = F hat x_{k-1|k-1} +Bu_k x^k∣k−1​=Fx^k−1∣k−1​+Buk​
计算误差协方差
P k ∣ k − 1 = F P k − 1 ∣ k − 1 F T + Q k P_{k|k-1} = FP_{k-1|k-1}F^T +Q_k Pk∣k−1​=FPk−1∣k−1​FT+Qk​

更新/校正阶段

计算残差
y ~ k = z k − H x ^ k ∣ k − 1 tilde y_k = z_k - H hat x_{k|k-1} y~​k​=zk​−Hx^k∣k−1​
计算残差协方差
S k = H P k ∣ k − 1 H T + R S_k = HP_{k|k-1}H^{T} +R Sk​=HPk∣k−1​HT+R
计算增益(置信度)
K k = P k ∣ k − 1 H T S k − 1 K_k = P_{k|k-1}H^TS_k^{-1} Kk​=Pk∣k−1​HTSk−1​

版权声明:该文观点仅代表作者本人。处理文章:请发送邮件至 三1五14八八95#扣扣.com 举报,一经查实,本站将立刻删除。