卡尔曼滤波EKF

目录

一、概述

二、卡尔曼滤波的5个公式

三、应用案例:汽车运动

四、应用案例:温度估计

五、总结


一、概述

初学者对于卡尔曼滤波5个公式有点懵,本文先接地气地介绍5个公式,然后举两个常用例子加强理解,同时附有Matlab代码。卡尔曼滤波在大学课程《现代控制理论》当中有涉及详细讲解。卡尔曼滤波使用条件有:

  • 1、线性系统;
  • 2、系统中噪声(不确定性)服从高斯分布。下文中的方差、误差、偏差、协方差都指不确定性的意思。误差 = 偏差;方差 = 偏差 ²。

在无人驾驶导航定位当中,需要多传感器对汽车位姿进行检测 ;在多传感器融合方面,使用卡尔曼滤波理论较多。对预测公式新的理解:第一个公式求解的是状态变量的期望值 。第二个公式求解的是状态变量的方差。

二、卡尔曼滤波的5个公式

以下内容有手写word,都是原创哈。

三、应用案例:汽车运动

完整的matlab代码:

Matlab 复制代码
clear all;
close all;
clc;
Z = (1:100); %观测值
noise = randn(1,100);%1行100列高斯白噪声
Z = Z + noise;

X = [0;0];%状态值
P = [1 0; 0 1];%状态协方差矩阵
F = [1 1; 0 1];%状态转移矩阵
Q = [0.0001, 0; 0 0.0001];%状态转移协方差矩阵
H = [1 0];%观测矩阵
R = 1;%观测噪声方差

figure;
%hold on;
speed = [];
distance = [];
for i =1:100
   %% 预测
   X_ = F*X;
   P_ = F*P*F' + Q;
   %% 更新
   K = P_*H'/(H*P_*H' + R);
   X = X_ + K*(Z(i) - H*X_);
   P = (eye(2) - K*H)*P_;

   speed(i) = X(2);
   distance(i) = X(1);
   %%plot(X(1), X(2));
end
plot(distance, speed);

四、应用案例:温度估计

五、总结

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

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 普遍认为不可适用于大型场景。
相关推荐
意.远3 分钟前
PyTorch参数管理详解:从访问到初始化与共享
人工智能·pytorch·python·深度学习
非优秀程序员12 分钟前
分享 | 我遇到的质量最高的MCP服务器汇总
人工智能
Sui_Network19 分钟前
Webacy 利用 Walrus 技术构建链上风险分析决策层
人工智能·游戏·web3·去中心化·区块链
振鹏Dong23 分钟前
字符串——面试考察高频算法题
java·数据结构·算法
leo030836 分钟前
详解相机的内参和外参,以及内外参的标定方法
计算机视觉·机器人·相机标定
知来者逆1 小时前
计算机视觉——为什么 mAP 是目标检测的黄金标准
图像处理·人工智能·深度学习·目标检测·计算机视觉
MobiCetus1 小时前
Deep Reinforcement Learning for Robotics翻译解读2
人工智能·深度学习·神经网络·机器学习·生成对抗网络·计算机视觉·数据挖掘
longlong int1 小时前
【每日算法】Day 17-1:位图(Bitmap)——十亿级数据去重与快速检索的终极方案(C++实现)
开发语言·c++·算法
师范大学生1 小时前
基于LSTM的文本分类2——文本数据处理
人工智能·rnn·lstm
泛舟起晶浪1 小时前
大衣的旅行--前缀和+二分
数据结构·算法