卡尔曼滤波与粒子滤波比较

最新推荐文章于 2024-06-16 23:54:11 发布

Lucas_Wang_Robot

最新推荐文章于 2024-06-16 23:54:11 发布

阅读量1.1w

点赞数 4

分类专栏： Robotics Programming Tools 文章标签：卡尔曼滤波粒子滤波

本文链接：https://blog.csdn.net/lump_night5/article/details/39379169

版权

两种滤波的重要性不必赘述，在自动控制领域的无异于牛顿的三大定律，都是贝叶斯滤波的不同表述和推广。

对卡尔曼滤波，[1]是状态估计的经典教材，它的第一章从简单的例子出发，深入浅出地解释什么是滤波，为什么卡尔曼滤波是最优的；粒子滤波强烈推荐[3]，这篇论文详细的介绍了例子滤波的发展历程和各类变种，最有用的是有人根据这篇论文实现了代码[3]；[4]是博士论文的节选，从编程的角度介绍了粒子滤波如何应用到移动机器人定位。

采用简单的线性模型，[ x vx y vy]'为状态变量，观测量是位置[ x y ]'，跟踪位置x的结果如下，注意到KF要比PF的跟踪效果好（PF的RMS大概是KF的四倍），这是因为从误差最小的原则来讲，Kalman Filter是最优的。

源代码：

Kalman Filter

%% Lucas Wang, XJTU, 2014.09.18, and I appreciate for the protype code offering by my girl friend, I love her very much~
clc;clear all;close all;
%% parameter of state equation
T = 0.01;
F=[1 T 0 0;
    0 1 0 0;
    0 0 1 T;
    0 0 0 1];
G=[T^2/2 0;
    T 0;
    0 T^2/2;
    0 T];
Q = diag( [0.04 0.09] );
%parameter of measurement equation
H=[1 0 0 0;
    0 0 1 0];
R=diag([900 900]);

% In