1.什么是卡尔曼滤波

先举个例子说一下什么是卡尔曼滤波。

假如有一个小机器人可以在草地上自由的运动。为了让它实现导航，机器人需要知道自己所处的位置。那么这个小机器人如何才能知道自己在什么位置呢？有两种方式：

根据起始位置及自身的运动进行运动学计算，从而得到自身的位置信息。
根据自身携带的传感器测量自身的位置信息，如GPS。

那么，现在就存在一个问题。不论是根据运动学计算还是利用传感器信息，得到的位置信息都不可避免的存在误差。那机器人到底该相信哪个数据呢？有或者，如何根据这两种数据来确定自身的位置呢？最简单的方式就是取平均值，但是这种方式合理吗？凭什么每一项的权重都是0.5？显然这种方式不合适。

既然，取均值的方式不合理，那又该怎么做呢？卡尔曼滤波解决的就是这个问题！

卡尔曼滤波的本质就是根据”测量值”（如GPS数据）和 “预测量”（如运动学计算结果）及 “误差”，计算得到当前状态的最优量。

2.卡尔曼滤波的数学推导

2.1 状态方程和测量方程

首先假设我们知道一个线性系统的状态差分方程为(1)xk=Axk−1+Buk−1+wk−1x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1} \tag{1}xk=Axk−1+Buk−1+wk−1(1) 其中 xxx 是系统的状态向量，大小为n*1列。AAA为转换矩阵，大小为 n∗nn*nn∗n。uuu为系统输入，大小为k∗1k*1k∗1。BBB是将输入转换为状态的矩阵，大小为n∗kn*kn∗k。随机变量www为系统噪声。

这里说句题外话，什么是状态方程？说的通俗一点，就是预测方程。根据上一时刻的状态及控制量来预测当前时刻的状态，当然，这个过程中不可避免的存在误差。

系统的测量方程为(2)zk=Hxk+vkz_k=Hx_k+v_k\tag{2}zk=Hxk+vk(2) zzz是测量值，大小为m∗1m*1m∗1，HHH也是状态变量到测量的转换矩阵。大小为m∗nm*nm∗n。随机变量vvv是测量噪声。

那什么又是测量方程呢？就是根据此刻的状态量及噪声得到的测量值的过程。另一方面测量方程也表明，测量值是由我们的状态值确定的。

对于状态方程中的系统噪声www和测量噪声vvv，假设服从如下多元高斯分布，并且www,vvv是相互独立的。其中QQQ,RRR为噪声变量的协方差矩阵。

2.2 卡尔曼滤波过程

在此，我们设xk^′\hat{x_k}'xk^′为预测值（根据上一时刻的状态及控制量由状态方程得到的当前时刻的状态）、xk^\hat{x_k}xk^为估计值（即卡尔曼滤波的最终的结果）、zk^\hat{z_k}zk^是测量值的预测（即，根据预测的状态值xk^′\hat{x_k}'xk^′得到的测量值）。则三者的关系为(3)xk^=xk^′+K(zk−zk^)=xk^′+K(zk−Hxk^′)\hat{x_k} = \hat{x_k}'+K(z_k-\hat{z_k})=\hat{x_k}'+K(z_k-H\hat{x_k}') \tag{3}xk^=xk^′+K(zk−zk^)=xk^′+K(zk−Hxk^′)(3)

其中，(zk−Hxk^′)(z_k-H\hat{x_k}')(zk−Hxk^′)称之为残差，也就是预测的和你实际测量值之间的差距。如果这项等于0，说明预测和测量出的完全吻合。

这个公式说明什么呢？他的意思就是最终的估计值等于预测值加上乘以系数KKK的测量残差。

从这个公式可以看出，我们可以根据状态方程得到预测值，根据测量方程可以得到测量值的预测，然后在根据公式3就可以得到最终的估计值。还有一个问题就是系数KKK的确定。

××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××

接下来说一下参数KKK的确定。

首先来看一下真实值与估计值之间协方差
(4)PK=E(ekekT)=E[(xk−xk^)(xk−xk^)T]P_K = E(e_ke_k^T) = E[(x_k-\hat{x_k})(x_k-\hat{x_k})^T] \tag{4} PK=E(ekekT)=E[(xk−xk^)(xk−xk^)T](4)
将公式3中的估计值带入可得
(5)PK=E[(xk−xk^′−K(zk−zk^))(xk−xk^′−K(zk−zk^))T]=E[(xk−xk^′−K(Hxk+vk−Hxk^′))(xk−xk^′−K(Hxk+vk−Hxk^′))T]=E[((I−KH)xk−xk^′−Kvk+KHxk^′)((I−KH)xk−xk^′−Kvk+KHxk^′)T]=E[((I−KH)(xk−xk^′)−Kvk)((I−KH)(xk−xk^′)−Kvk)T]P_K = E[(x_k-\hat{x_k}'-K(z_k-\hat{z_k}))(x_k-\hat{x_k}'-K(z_k-\hat{z_k}))^T] \\ = E[(x_k-\hat{x_k}'-K(Hx_k+v_k-H\hat{x_k}'))(x_k-\hat{x_k}'-K(Hx_k+v_k-H\hat{x_k}'))^T] \\ = E[((I-KH)x_k-\hat{x_k}'-Kv_k+KH\hat{x_k}')((I-KH)x_k-\hat{x_k}'-Kv_k+KH\hat{x_k}')^T] \\ = E[((I-KH)(x_k-\hat{x_k}')-Kv_k)((I-KH)(x_k-\hat{x_k}')-Kv_k)^T] \tag{5} PK=E[(xk−xk^′−K(zk−zk^))(xk−xk^′−K(zk−zk^))T]=E[(xk−xk^′−K(Hxk+vk−Hxk^′))(xk−xk^′−K(Hxk+vk−Hxk^′))T]=E[((I−KH)xk−xk^′−Kvk+KHxk^′)((I−KH)xk−xk^′−Kvk+KHxk^′)T]=E[((I−KH)(xk−xk^′)−Kvk)((I−KH)(xk−xk^′)−Kvk)T](5)

同理，预测值与真实值之间的写方差矩阵为
(6)PK′=E[ek′ek′T]=E[(xk−xk^′)(xk−xk^′)T]=E[(A(xk−1−xk−1^)+wk−1)(A(xk−1−xk−1^)+wk−1)T]=E[(Aek−1)(Aek−1)T]+E[wk−1wk−1T]=APk−1AT+QP_K' = E[e_k'e_k'^T] = E[(x_k-\hat{x_k}')(x_k-\hat{x_k}')^T] \\=E[(A(x_{k-1}-\hat{x_{k-1}})+w_{k-1})(A(x_{k-1}-\hat{x_{k-1}})+w_{k-1})^T] \\=E[(Ae_{k-1})(Ae_{k-1})^T] +E[w_{k-1}w_{k-1}^T] = AP_{k-1}A^T+Q\tag{6}PK′=E[ek′ek′T]=E[(xk−xk^′)(xk−xk^′)T]=E[(A(xk−1−xk−1^)+wk−1)(A(xk−1−xk−1^)+wk−1)T]=E[(Aek−1)(Aek−1)T]+E[wk−1wk−1T]=APk−1AT+Q(6)
系统状态x变量和测量噪声之间是相互独立的。所以，公式5可以写为
(7)PK=E[((I−KH)(xk−xk^′)−Kvk)((I−KH)(xk−xk^′)−Kvk)T]=(I−KH)E[(xk−xk^′)(xk−xk^′)T](I−KH)T+KE[vkvkT]KTP_K = E[((I-KH)(x_k-\hat{x_k}')-Kv_k)((I-KH)(x_k-\hat{x_k}')-Kv_k)^T] \\ =(I-KH)E[(x_k-\hat{x_k}')(x_k-\hat{x_k}')^T](I-KH)^T+KE[v_kv_k^T]K^T \tag{7} PK=E[((I−KH)(xk−xk^′)−Kvk)((I−KH)(xk−xk^′)−Kvk)T]=(I−KH)E[(xk−xk^′)(xk−xk^′)T](I−KH)T+KE[vkvkT]KT(7)

将6式带入7式可得
(8)PK=(I−KH)PK′(I−KH)T+KE[vkvkT]KT=PK′−KHPk′−PK′HTKT+K(HPk′HT+R)KTP_K =(I-KH)P_K'(I-KH)^T+KE[v_kv_k^T]K^T = P_K'-KHP_k'-P_K'H^TK^T +K(HP_k'H^T+R)K^T\tag{8} PK=(I−KH)PK′(I−KH)T+KE[vkvkT]KT=PK′−KHPk′−PK′HTKT+K(HPk′HT+R)KT(8)

协方差矩阵的对角线元素就是方差。这样一来，把矩阵P−KP-KP−K的对角线元素求和，用字母T来表示这种算子，他的学名叫矩阵的迹。则有
(9)T[PK]=T[PK′]−T[KHPk′]−T[PK′HTKT]+T[K(HPk′HT+R)KT]T[P_K] = T[P_K']-T[KHP_k']-T[P_K'H^TK^T] +T[K(HP_k'H^T+R)K^T]\tag{9} T[PK]=T[PK′]−T[KHPk′]−T[PK′HTKT]+T[K(HPk′HT+R)KT](9)

这样，我们就得到了关于K的一个方程。对其求导可得：

令导数为0，可以求得：
(10)K=Pk′HT(HPk′HT+R)−1K =P_k'H^T (HP_k'H^T+R)^{-1}\tag{10}K=Pk′HT(HPk′HT+R)−1(10)

将K值带入公式8中，可得：
(11)PK=(I−KH)Pk′P_K = (I-KH)P_k'\tag{11}PK=(I−KH)Pk′(11)
×××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××

至此，推到过程结束。我们来理一下卡尔曼滤波的思路：

1.首先，根据公式1计算我们的预测值，根据公式计算预测值与真实值的协方差矩阵xk^′=Ax^k−1+Buk−1\hat{x_k}' = A\hat{x}_{k-1}+Bu_{k-1}xk^′=Ax^k−1+Buk−1 PK′=APk−1AT+QP_K' = AP_{k-1}A^T+QPK′=APk−1AT+Q
2.根据1中得到的数据计算卡尔曼增益KKK K=Pk′HT(HPk′HT+R)−1K =P_k'H^T (HP_k'H^T+R)^{-1}K=Pk′HT(HPk′HT+R)−1
3.根据预测值和测量值以及卡尔曼增益来计算最终的结果xk^=xk^′+K(zk−Hxk^′)\hat{x_k} =\hat{x_k}'+K(z_k-H\hat{x_k}')xk^=xk^′+K(zk−Hxk^′)
4.最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。PK=(I−KH)Pk′P_K = (I-KH)P_k'PK=(I−KH)Pk′

卡尔曼滤波过程就是不断利用以上5个公式进行计算的过程。

3 卡尔曼滤波应用

此部分内容来自：https://blog.csdn.net/m0_38089090/article/details/79523784

应用背景是匀加速小车，该线性系统的状态差分方程为xk=Axk−1+Buk−1+wk−1x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}xk=Axk−1+Buk−1+wk−1
对小车进行建模，ft为合力，小车的状态方程表示为

矩阵形式表示为

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <eigen3/Eigen/Dense>//包含Eigen矩阵运算库，用于矩阵计算
#include <cmath>
#include <limits>//用于生成随机分布数列using namespace std;
using Eigen::MatrixXd;double generateGaussianNoise(double mu, double sigma);//随机高斯分布数列生成器函数int main(int argc, char* argv[])
{//""中是txt文件路径，注意：路径要用//隔开ofstream fout("..//result.txt");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();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;
}

测试结果为：

我所理解的卡尔曼滤波——公式推导与应用相关推荐

卡尔曼滤波公式推导（2）
上一篇文章从概率密度函数的角度推导了卡尔曼滤波公式(卡尔曼滤波公式推导(1)),接下来从矩阵的最小二乘法的角度来推导. 在预测和更新阶段分别能得到两个近似状态向量真实值的值,记为x^predict=x ...
【卡尔曼滤波】我所理解的卡尔曼滤波
卡尔曼滤波在我当学生的时候就用过,但是当年我似乎就是套公式,没有理解其精髓,加之时间久了有点模糊,突然需要指导学生使用,有了强烈的陌生感觉,不得不逼自己再一次捡起.自己学会和教会别人是学习的两个层次, ...
卡尔曼滤波---公式推导和一些疑问
该笔记是在学习up主DR_CAN的关于卡尔曼滤波视频后做的笔记整理. up主主页:https://space.bilibili.com/230105574 文章中有三个链接,补充如下: 贝叶斯滤波-- ...
从理论到实战-如何理解那个把嫦娥送上天的卡尔曼滤波算法Kalman filter?
文章目录直观理解卡尔曼滤波怎么做的? 卡尔曼滤波Python代码实践原文链接:https://zhuanlan.zhihu.com/p/77327349 直观理解 **首先卡尔曼滤波要解决的问题 ...
通俗理解卡尔曼滤波（无人驾驶感知融合的经典算法）
前言我个人有近10年AI教育经验了,中间获得过一些名号,比如北理工校外导师,微软MVP兼CSDN技术专家,本博客也有1700多万PV了,在AI圈内有极高知名度.后2015年和团队一块创业创办AI职教 ...
卡尔曼滤波MATLAB代码实现
没有大量的公式推导,个人感觉也没有必要,我们从小推导过很多公式,试着想想我们还能回忆起几个?个人认为只需要记住公式的用法,作用,知道有这个公式就可以.用的时候我们可以随时去查.所以楼主参考网上资料结合 ...
【学习笔记】卡尔曼滤波中的协方差矩阵
本文转载至: 添加一些自己的理解和标注的重点,算是学习笔记吧: 一. 什么是卡尔曼滤波: 首先定义问题: 对于某一个系统,知道当前状态Xt,存在以上两个问题: 1. 经过时间△t后,下一个状态如何求出 ...
关于卡尔曼滤波和粒子滤波最直白的解释
卡尔曼滤波本来是控制系统课上学的,当时就没学明白,也蒙混过关了,以为以后也不用再见到它了,可惜没这么容易,后来学计算机视觉和图像处理,发现用它的地方更多了,没办法的时候只好耐心学习和理解了.一直很想把 ...
一文透彻详解卡尔曼滤波原理
文章目录前言一卡尔曼滤波中的五个公式二重要公式与变量的理解三卡尔曼滤波的一个简单demo 前言由于最近一段时间在做多传感器融合工作,以下是根据这段时间的实践与学习,对卡尔曼滤波的一些理 ...

我所理解的卡尔曼滤波——公式推导与应用