《卡尔曼滤波》 一、简介及例子

2019/01/02 19:48
阅读数 127

    卡尔曼滤波在大学课程《现代控制理论》当中有涉及详细讲解。卡尔曼滤波使用条件有:1、线性系统; 2、系统中噪声(不确定性)服从高斯分布。下文中的方差、误差、偏差、协方差都指不确定性的意思。误差 = 偏差;方差 = 偏差 ²。在无人驾驶导航定位当中,需要多传感器对汽车位姿进行检测 ;在多传感器融合方面,使用卡尔曼滤波理论较多。

1、5个公式

 

 

 2、汽车例子

 

 1 clear all;
 2 close all;
 3 clc;
 4 Z = (1:100); %观测值
 5 noise = randn(1,100);%1行100列高斯白噪声
 6 Z = Z + noise; 
 7 
 8 X = [0;0];%状态值
 9 P = [1 0; 0 1];%状态协方差矩阵
10 F = [1 1; 0 1];%状态转移矩阵
11 Q = [0.0001, 0; 0 0.0001];%状态转移协方差矩阵
12 H = [1 0];%观测矩阵
13 R = 1;%观测噪声方差
14 
15 figure;
16 %hold on;
17 speed = [];
18 distance = [];
19 for i =1:100
20    %% 预测
21    X_ = F*X;
22    P_ = F*P*F' + Q;
23    %% 更新
24    K = P_*H'/(H*P_*H' + R);
25    X = X_ + K*(Z(i) - H*X_);
26    P = (eye(2) - K*H)*P_;
27 
28    speed(i) = X(2);
29    distance(i) = X(1);
30    %%plot(X(1), X(2));
31 end
32 plot(distance, speed);

 

3、温度估计例子

 

       在线性高斯系统中,卡尔曼滤波器构成了该系统中的最大后验概率估计。而且,由于高斯分布经过线性变换后仍服从高斯分布,所以整个过程中我们没有进行任何的近似。可以说,卡尔曼滤波器构成了线性系统的最优无偏估计

       SLAM 中的运动方程和观测方程通常是非线性函数,尤其是视觉 SLAM 中的相机模型,需要使用相机内参模型以及李代数表示的位姿,更不可能是一个线性系统。一个高斯分布,经过非线性变换后,往往不再是高斯分布,所以在非线性系统中,我们必须取一定的近似,将一个非高斯的分布近似成一个高斯分布。我们希望把卡尔曼滤波器的结果拓展到非线性系统中来,称为扩展卡尔曼滤波器(Ex-tended Kalman Filter,EKF)。通常的做法是,在某个点附近考虑运动方程以及观测方程的一阶泰勒展开,只保留一阶项,即线性的部分,然后按照线性系统进行推导。

EKF的局限性:

1.    首先,滤波器方法在一定程度上假设了马尔可夫性,也就是 k 时刻的状态只与 k −1时刻相关,而与 k − 1 之前的状态和观测都无关(或者和前几个有限时间的状态相关)。这有点像是在视觉里程计中,只考虑相邻两帧关系一样。如果当前帧确实与很久之前的数据有关(例如回环),那么滤波器就会难以处理这种情况。而非线性优化方法则倾向于使用所有的历史数据。它不光考虑邻近时刻的特征点与轨迹关系,更会把考虑很久之前的状态也考虑进来,称为全体时间上的 SLAM(Full-SLAM)。在这种意义下,非线性优化方法使用了更多信息,当然也需要更多的计算。

2.与第六章介绍的优化方法相比,EKF 滤波器仅在 k−1时刻相机位姿处做了一次线性化,然后就直接根据这次线性化结果,把后验概率给算了出来。这相当于在说,我们认为该点处的线性化近似,在后验概率处仍然是有效的。而实际上,当我们离开工作点较远的时候,一阶泰勒展开并不一定能够近似整个函数,这取决于运动模型和观测模型的非线性情况。如果它们有强烈的非线性,那线性近似就只在很小范围内成立,不能认为在很远的地方仍能用线性来近似。这就是 EKF 的非线性误差,是它的主要问题所在。在优化问题中,尽管我们也做一阶(最速下降)或二阶(G-N 或 L-M)的近似,但每迭代一次,状态估计发生改变之后,我们会重新对新的估计点做泰勒展开,而不像EKF 那样只在固定点上做一次泰勒展开。这就导致优化方法适用范围更广,则在状态变化较大时亦能适用。

3.从程序实现上来说,EKF 需要存储状态量的均值和方差,并对它们进行维护和更新。如果把路标也放进状态的话,由于视觉 SLAM 中路标数量很大,这个存储量是相当可观的,且与状态量呈平方增长(因为要存储协方差矩阵)。因此,EKF SLAM 普遍认为不可适用于大型场景。

 

 参考:https://blog.csdn.net/u010720661/article/details/63253509

 

展开阅读全文
打赏
0
0 收藏
分享
加载中
更多评论
打赏
0 评论
0 收藏
0
分享
返回顶部
顶部