图说卡尔曼滤波(C++实现)
什么是卡尔曼滤波?
对于这个滤波器,我们几乎可以下这么一个定论:只要是存在不确定信息的动态系统,卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰,卡尔曼滤波通常也能很好的弄清楚究竟发生了什么,找出现象间不易察觉的相关性。
因此卡尔曼滤波非常适合不断变化的系统,它的优点还有内存占用较小(只需保留前一个状态)、速度快,是实时问题和嵌入式系统的理想选择。
如果你曾经Google过卡尔曼滤波的教程(如今有一点点改善),你会发现相关的算法教程非常可怕,而且也没具体说清楚是什么。事实上,卡尔曼滤波很简单,如果我们以正确的方式看它,理解是很水到渠成的事。
本文会用大量清晰、美观的图片和颜色来解释这个概念,读者只需具备概率论和矩阵的一般基础知识。
我们能用卡尔曼滤波做什么?
让我们举个例子:你造了一个可以在树林里四处溜达的小机器人,为了让它实现导航,机器人需要知道自己所处的位置。
也就是说,机器人有一个包含位置信息和速度信息的状态 :
注意,在这个例子中,状态是位置和速度,放进其他问题里,它也可以是水箱里的液体体积、汽车引擎温度、触摸板上指尖的位置,或者其他任何数据。
我们的小机器人装有GPS传感器,定位精度10米。虽然一般来说这点精度够用了,但我们希望它的定位误差能再小点,毕竟树林里到处都是土坑和陡坡,如果机器人稍稍偏了那么几米,它就有可能滚落山坡。所以GPS提供的信息还不够充分。
我们也可以预测机器人是怎么移动的:它会把指令发送给控制轮子的马达,如果这一刻它始终朝一个方向前进,没有遇到任何障碍物,那么下一刻它可能会继续坚持这个路线。但是机器人对自己的状态不是全知的:它可能会逆风行驶,轮子打滑,滚落颠簸地形……所以车轮转动次数并不能完全代表实际行驶距离,基于这个距离的预测也不完美。
这个问题下,GPS为我们提供了一些关于状态的信息,但那是间接的、不准确的;我们的预测提供了关于机器人轨迹的信息,但那也是间接的、不准确的。
但以上就是我们能够获得的全部信息,在它们的基础上,我们是否能给出一个完整预测,让它的准确度比机器人搜集的单次预测汇总更高?用了卡尔曼滤波,这个问题可以迎刃而解。
卡尔曼滤波眼里的机器人问题
还是上面这个问题,我们有一个状态,它和速度、位置有关:
我们不知道它们的实际值是多少,但掌握着一些速度和位置的可能组合,其中某些组合的可能性更高:
卡尔曼滤波假设两个变量(在我们的例子里是位置和速度)都应该是随机的,而且符合高斯分布。每个变量都有一个均值 ,它是随机分布的中心;有一个方差 ,它衡量组合的不确定性。
在上图中,位置和速度是不相关的,这意味着我们不能从一个变量推测另一个变量。
那么如果位置和速度相关呢?如下图所示,机器人前往特定位置的可能性取决于它拥有的速度。
这不难理解,如果基于旧位置估计新位置,我们会产生这两个结论:如果速度很快,机器人可能移动得更远,所以得到的位置会更远;如果速度很慢,机器人就走不了那么远。
这种关系对目标跟踪来说非常重要,因为它提供了更多信息:一个可以衡量可能性的标准。这就是卡尔曼滤波的目标:从不确定信息中挤出尽可能多的信息!
为了捕获这种相关性,我们用的是协方差矩阵。简而言之,矩阵的每个值是第 个变量和第 个变量之间的相关程度(由于矩阵是对称的, 和 的位置可以随便交换)。我们用 表示协方差矩阵,在这个例子中,就是 。
用矩阵描述问题
为了把以上关于状态的信息建模为高斯分布(图中色块),我们还需要 时的两个信息:最佳估计 (均值,也就是 ),协方差矩阵 。(虽然还是用了位置和速度两个变量,但只要和问题相关,卡尔曼滤波可以包含任意数量的变量)
接下来,我们要通过查看当前状态(k-1时)来预测下一个状态(k时)。这里我们查看的状态不是真值,但预测函数无视真假,可以给出新分布:
我们可以用矩阵 表示这个预测步骤:
它从原始预测中取每一点,并将其移动到新的预测位置。如果原始预测是正确的,系统就会移动到新位置。
这是怎么做到的?为什么我们可以用矩阵来预测机器人下一刻的位置和速度?下面是个简单公式:
换成矩阵形式:
这是一个预测矩阵,它能给出机器人的下一个状态,但目前我们还不知道协方差矩阵的更新方法。这也是我们要引出下面这个等式的原因:如果我们将分布中的每个点乘以矩阵A,那么它的协方差矩阵会发生什么变化
把这个式子和上面的最佳估计 结合,可得:
外部影响
但是,除了速度和位置,外因也会对系统造成影响。比如模拟火车运动,除了列车自驾系统,列车操作员可能会手动调速。在我们的机器人示例中,导航软件也可以发出停止指令。对于这些信息,我们把它作为一个向量 ,纳入预测系统作为修正。
假设油门设置和控制命令是已知的,我们知道火车的预期加速度 。根据运动学基本定理,我们可得:
把它转成矩阵形式:
是控制矩阵, 是制向量。如果外部环境异常简单,我们可以忽略这部分内容,但是如果添加了外部影响后,模型的准确率还是上不去,这又是为什么呢?
外部不确定性
如果状态根据自己的属性发展,一切都很好。 如果状态根据外力发展,只要我们知道那些外部力量是什么,一切都仍然很好。
但是,如果存在我们不知道的力量呢?当我们监控无人机时,它可能会受到风的影响;当我们跟踪轮式机器人时,它的轮胎可能会打滑,或者粗糙地面会降低它的移速。这些因素是难以掌握的,如果出现其中的任意一种情况,预测结果就难以保障。
这要求我们在每个预测步骤后再加上一些新的不确定性,来模拟和“世界”相关的所有不确定性:
如上图所示,加上外部不确定性后, 的每个预测状态都可能会移动到另一点,也就是蓝色的高斯分布会移动到紫色高斯分布的位置,并且具有协方差 。换句话说,我们把这些不确定影响视为协方差 的噪声。
这个紫色的高斯分布拥有和原分布相同的均值,但协方差不同。
我们在原式上加入 :
简而言之,这里:
是基于 和 校正后得到的预测。
是基于 和 得到的预测。
现在,有了这些概念介绍,我们可以把传感器数据输入其中。
通过测量来细化估计值
我们可能有好几个传感器,它们一起提供有关系统状态的信息。传感器的作用不是我们关心的重点,它可以读取位置,可以读取速度,重点是,它能告诉我们关于状态的间接信息——它是状态下产生的一组读数。
请注意,读数的规模和状态的规模不一定相同,所以我们把传感器读数矩阵设为 。
把这些分布转换为一般形式:
卡尔曼滤波的一大优点是擅长处理传感器噪声。换句话说,由于种种因素,传感器记录的信息其实是不准的,一个状态事实上可以产生多种读数。
从我们观察到的每个读数中,我们可能会猜测我们的系统处于特定状态。但由于存在不确定性,一些状态比其他状态更有可能产生我们所看到的读数:
我们将这种不确定性(即传感器噪声)的协方差设为 ,读数的分布均值设为 。
现在我们得到了两块高斯分布,一块围绕预测的均值,另一块围绕传感器读数。
如果要生成靠谱预测,模型必须调和这两个信息。也就是说,对于任何可能的读数 ,这两种方法预测的状态都有可能是准的,也都有可能是不准的。重点是我们怎么找到这两个准确率。
最简单的方法是两者相乘:
两块高斯分布相乘后,我们可以得到它们的重叠部分,这也是会出现最佳估计的区域。换个角度看,它看起来也符合高斯分布:
事实证明,当你把两个高斯分布和它们各自的均值和协方差矩阵相乘时,你会得到一个拥有独立均值和协方差矩阵的新高斯分布。最后剩下的问题就不难解决了:我们必须有一个公式来从旧的参数中获取这些新参数!
结合高斯
让我们从一维看起,设方差为 ,均值为 ,一个标准一维高斯钟形曲线方程如下所示:
那么两条高斯曲线相乘呢?
把这个式子按照一维方程进行扩展,可得:
如果有些太复杂,我们用k简化一下:
以上是一维的内容,如果是多维空间,把这个式子转成矩阵格式:
这个矩阵 就是我们说的卡尔曼增益,easy!
把它们结合在一起
截至目前,我们有用矩阵 预测的分布,有用传感器读数 预测的分布。把它们代入上节的矩阵等式中:
相应的,卡尔曼增益就是:
考虑到 里还包含着一个 ,我们再精简一下上式:
最后, 是我们的最佳估计值,我们可以把它继续放进去做另一轮预测:
应用例子:估计小车的运动状态
1、数学模型:
有一个匀加速运动的小车,它受到的合力为ft,由匀加速运动的位移和速度公式,能得到由t-1到t时刻的位移和速度变化公式:
该系统的状态向量包括位移和速度,分别用xt和xt的导数表示。控制输入变量为u,也就是加速度,于是有如下形式:
所以这个系统的状态方程为:
这里对应的矩阵A大小为2*2,矩阵B大小为2*1.
注意Z是测量值,大小为m*1,H也是状态变量到测量的转换矩阵,大小为m*n。随机变量v是测量噪声。
假设车是匀加速远离我们,站在原点用超声波测量小车离我们的距离。
对于小车匀加速运动的模型,假设系统的噪声向量只存在速度分量上,且速度噪声的方差是一个常量0.01,位移分量上的系统噪声为0.测量值只有位移,它的协方差矩阵大小是1*1,就是测量噪声的方差本身。
Q中,叠加在速度上系统噪声方差为0.01,位移上的为0,它们间的协方差为0,即噪声间没有关联。
2、C++实现代码:
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <Eigen/Dense>//包含Eigen矩阵运算库,用于矩阵计算
#include <cmath>
#include <limits>//用于生成随机分布数列using namespace std;
using Eigen::MatrixXd;int main(int argc, char **argv[])
{//""中是txt文件路径,注意:路径要用//隔开ofstream fout("..//result.txt");double generateGaussianNoise(double mu, double sigma);//随机高斯分布数列生成器函数const double delta_t = 0.1;//控制周期,100msconst int num = 100;//迭代次数const double acc = 10;//加速度,ft/mMatrixXd A(2, 2);A(0, 0) = 1;A(1, 0) = 0;A(0, 1) = delta_t;A(1, 1) = 1;MatrixXd B(2, 1);B(0, 0) = pow(delta_t, 2) / 2;B(1, 0) = delta_t;MatrixXd H(1, 2);//测量的是小车的位移,速度为0H(0, 0) = 1;H(0, 1) = 0;MatrixXd Q(2, 2);//过程激励噪声协方差,假设系统的噪声向量只存在速度分量上,且速度噪声的方差是一个常量0.01,位移分量上的系统噪声为0Q(0, 0) = 0;Q(1, 0) = 0;Q(0, 1) = 0;Q(1, 1) = 0.01;MatrixXd R(1, 1);//观测噪声协方差,测量值只有位移,它的协方差矩阵大小是1*1,就是测量噪声的方差本身。R(0, 0) = 10;//time初始化,产生时间序列vector<double> time(100, 0);for (decltype(time.size()) i = 0; i != num; ++i) {time[i] = i * delta_t;//cout<<time[i]<<endl;}MatrixXd X_real(2, 1);vector<MatrixXd> x_real, rand;//生成高斯分布的随机数for (int i = 0; i<100; ++i) {MatrixXd a(1, 1);a(0, 0) = generateGaussianNoise(0, sqrt(10));rand.push_back(a);}//生成真实的位移值for (int i = 0; i < num; ++i) {X_real(0, 0) = 0.5 * acc * pow(time[i], 2);X_real(1, 0) = 0;x_real.push_back(X_real);}//变量定义,包括状态预测值,状态估计值,测量值,预测状态与真实状态的协方差矩阵,估计状态和真实状态的协方差矩阵,初始值均为零MatrixXd X_evlt = MatrixXd::Constant(2, 1, 0), X_pdct = MatrixXd::Constant(2, 1, 0), Z_meas = MatrixXd::Constant(1, 1, 0),Pk = MatrixXd::Constant(2, 2, 0), Pk_p = MatrixXd::Constant(2, 2, 0), K = MatrixXd::Constant(2, 1, 0);vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k;x_evlt.push_back(X_evlt);x_pdct.push_back(X_pdct);z_meas.push_back(Z_meas);pk.push_back(Pk);pk_p.push_back(Pk_p);k.push_back(K);//开始迭代for (int i = 1; i < num; ++i) {//预测值X_pdct = A * x_evlt[i - 1] + B * acc;x_pdct.push_back(X_pdct);//预测状态与真实状态的协方差矩阵,Pk'Pk_p = A * pk[i - 1] * A.transpose() + Q;pk_p.push_back(Pk_p);//K:2x1MatrixXd tmp(1, 1);tmp = H * pk_p[i] * H.transpose() + R;K = pk_p[i] * H.transpose() * tmp.inverse();k.push_back(K);//测量值zZ_meas = H * x_real[i] + rand[i];z_meas.push_back(Z_meas);//估计值X_evlt = x_pdct[i] + k[i] * (z_meas[i] - H * x_pdct[i]);x_evlt.push_back(X_evlt);//估计状态和真实状态的协方差矩阵,PkPk = (MatrixXd::Identity(2, 2) - k[i] * H) * pk_p[i];pk.push_back(Pk);}cout << "含噪声测量" << " " << "后验估计" << " " << "真值" << " " << endl;for (int i = 0; i < num; ++i) {cout<<z_meas[i]<<" "<<x_evlt[i](0,0)<<" "<<x_real[i](0,0)<<endl;fout << z_meas[i] << " " << x_evlt[i](0, 0) << " " << x_real[i](0, 0) << endl;//输出到txt文档,用于matlab绘图//cout<<k[i](1,0)<<endl;//fout<<rand[i](0,0)<<endl;//fout<<x_pdct[i](0,0)<<endl;}fout.close();getchar();return 0;
}//生成高斯分布随机数的函数,网上找的
double generateGaussianNoise(double mu, double sigma)
{const double epsilon = std::numeric_limits<double>::min();const double two_pi = 2.0*3.14159265358979323846;static double z0, z1;static bool generate;generate = !generate;if (!generate)return z1 * sigma + mu;double u1, u2;do{u1 = rand() * (1.0 / RAND_MAX);u2 = rand() * (1.0 / RAND_MAX);} while (u1 <= epsilon);z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);return z0 * sigma + mu;
}
3、MATLAB绘图:
a = dlmread('result.txt');
plot(a)
4、调参经验
?(待实践)
(a) 一开始预测不准,则更相信测量值,增大初始的Pk->增大Pk`->k->更相信测量值
(b) 状态方程建立不正确,相当于过程噪声增大了,所以这个时候应该增大过程噪声的方差Q
参考:
How a Kalman filter works, in pictures
图说卡尔曼滤波,一份通俗易懂的教
卡尔曼滤波 -- 从推导到应用
https://blog.csdn.net/m0_38089090/article/details/79523784
图说卡尔曼滤波(C++实现)相关推荐
- 图说卡尔曼滤波(正在进行时)
什么是卡尔曼滤波? 你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波器来对系统的下一步做出有根据的预测.即使有噪声信息的干扰,卡尔曼滤波器也能弄清楚真实发生的事情. 卡尔曼滤波器非常适合持续变化的 ...
- 通俗理解卡尔曼滤波(无人驾驶感知融合的经典算法)
前言 我个人有近10年AI教育经验了,中间获得过一些名号,比如北理工校外导师,微软MVP兼CSDN技术专家,本博客也有1700多万PV了,在AI圈内有极高知名度.后2015年和团队一块创业创办AI职教 ...
- 始卡尔曼滤波算法(KF)、扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别?
原始卡尔曼滤波算法(KF).扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别? 原文:https://www.zhihu.com/question/22714163/answ ...
- 经典重温:卡尔曼滤波器介绍与理论分析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨 lcl 来源丨我爱计算机视觉 最近业余在研究物体追踪,看到传统的方法用到了卡尔曼滤波(Kalm ...
- SORT 多目标跟踪算法笔记
SORT 是一种简单的在线实时多目标跟踪算法.文章要点为: 以 IoU 作为前后帧间目标关系度量指标: 利用卡尔曼滤波器预测当前位置: 通过匈牙利算法关联检测框到目标: 应用试探期甄别虚检: 使用 F ...
- 一文图解卡尔曼滤波(Kalman Filter)
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 译者注:这恐怕是全网有关卡尔曼滤波最简单易懂的解释,如果你认真的读 ...
- 图解卡尔曼滤波(Kalman Filter)
背景 关于滤波 首先援引来自知乎大神的解释. "一位专业课的教授给我们上课的时候,曾谈到:filtering is weighting(滤波即加权).滤波的作用就是给不同的信号分量不同的权重 ...
- 我所理解的卡尔曼滤波——公式推导与应用
我所理解的卡尔曼滤波--公式推导与应用 1.什么是卡尔曼滤波 2.卡尔曼滤波的数学推导 2.1 状态方程和测量方程 2.2 卡尔曼滤波过程 3 卡尔曼滤波应用 1.什么是卡尔曼滤波 先举个例子说一下什 ...
- 【camera-lidar-radar】基于卡尔曼滤波和扩展卡尔曼滤波的相机、激光雷达、毫米波雷达多传感器后融合
[camera-lidar-radar]基于卡尔曼滤波和扩展卡尔曼滤波的相机.激光雷达.毫米波雷达多传感器后融合 代码下载地址(C++ and Python):下载地址 红点和蓝点分别表示radar和 ...
最新文章
- android对话框的使用(下)
- [原创]Coding4Fun检测你的网络,用C#获取本机TCP、UDP状态及连接(二)
- linux rm(remove) 命令详解
- 010-ThreadGroup线程组
- 正则表达式之零宽断言
- Chrome无法播放m3u8格式的直播视频流的问题解决
- vuejs 轮播_如何在VueJS中设计和构建轮播功能
- 浅谈 CSRF 攻击方式
- 计算机专用英语词汇pdf,计算机专用英语词汇1500词.pdf
- SQLite | Insert、Delete、Updata 与 Drop 语句
- 【NOIP2004】【Luogu1085】不高兴的津津
- mfc 添加变量出现灾难性故障_实验室近期论文:储罐灾难性失效事故的漫堤预测模型...
- find命令结合exec和xargs使用的区别
- 百度要革自己的命?移动搜索或取消PC网页收录
- java - EM算法
- ul阻燃标准有几个等级_UL阻燃等级
- 如何将pdf转换成word的3种免费方法
- python贪吃蛇游戏代码详解外加中文_Python贪吃蛇游戏编写代码
- 再探阿里的“数据+业务”双中台架构
- Linux dd 命令 限制文件夹大小
热门文章
- windows7现实计算机内存不足,windows7内存不足怎么办
- Hadoop、Pig、Hive、Storm、NOSQL 学习资源收集【Updating】
- 修改手机服务器密码忘了怎么办啊,交管服务平台忘了密码而且换了手机号
- 辐射安全管理考试笔记
- 【C++算法题】求三角形边长
- linux ps 被替换文件怎么恢复,被覆盖的文件如何恢复_被替换掉的文件怎么恢复-win7之家...
- 大工18秋计算机1答案,大工18秋《专业英语(计算机英语)》在线作业1.docx
- 手机长按android版本,微信长按这个功能,可以触发高级操作,只有安卓手机才有...
- 字体转换网站——Font Squirrel(推荐阅读)
- java打印日期序列_Java8新特性之新日期API