浅析卡尔曼滤波器与扩展卡尔曼

前言：今天刚刚接触这个东西，本来打算看看教材，但是苦于没有学过现代控制理论之类的一些东西，再加上书上那些公式看着就头疼，所以打算看看csdn上的大神是怎么理解的哈哈，再看完后确实理解了不少，并且用c++也挺容易实现的。于是记录下来0.0

---

在介绍之前，我们先将卡尔曼现实化，这样好理解一些。

假设你在家里称体重，但是很不幸今天的称抽风了，导致称很不准，那么你获得今天的体重有两种方式，一种是根据今天的坏的称，还有就是根据昨天记录的昨天的体重，这里的你称得体重称为观测量y（k），你根据昨天的体重估计今天的体重称为估计量

x_(k)（预测量、状态值），你现在结合两者得到的值为**最优值**x(k)。

那这个估计量x_(k)怎么求呢？求他需要用到上一个时刻的最优值，公式是 x_(k) = A*x(k-1) + B*u_(k-1) + w_(t-1) ;这里u为系统控制变量，一般用不到，为零，A为一个矩阵， +w_(t-1)为噪声，有时没有，所以为x_(k) = A*x(k-1)。

另外，我们根据前面的x_(k) = A*x(k-1) + B*u_(k-1) + w_(t-1)求得的当前时刻的估计值他有一个的偏差P_(k) = A * P(k-1) * A^T + Q ,其中Q为过程噪声已知，P(k-1)为上一时刻最优值的偏差, A、B均为已知矩阵。

弄完了估计值，现在弄这个测量值，我们需要知道：观测值y(k)=H*x_(k) + R,其中H为已知的矩阵，R为噪声。

前面已经知道了观测值和估计值，那么这个最优值是多少呢？
公式： x(k) = x_(k) + K( y(k) - H*x_(k) ) ,其中K就是卡尔曼增益。这个公式也可以理解成是对x_(k) = A*x(k-1) + B*u_(k-1) + w_(t-1)得到的x_(k)的校正

接下来看看怎么求K：K = P_(k)* H^T * ( H * P_(k) * H^T + R) ^(-1);
把这里的K带入上面的公式就求出此刻的最优值x(k).

---

下面进入下一个时刻（k+1）时刻，在进入之前，我们再回顾上面的内容看需要哪些前一时刻的内容，以便于在这一时刻运用。我们发现有上一时刻的最优值以及上一时刻最优质的偏差，最优值已经有了，所以我们还差一个最优值的偏差，
为：P(k)= (I- K * H ) P_(k) ，这个公式也可以理解成是对前面的P_(k) = A * P(k-1) * A^T + Q得到的P_(k)进行矫正

ps：上面的公式中凡是带的，都是与估计值有关的量，比如x(k)为估计值，P_(k)为估计值的偏差。不带_的，都是与最优值有关的。

到这里就结束了，剩下的就是反复迭代了。

补充：1.卡尔慢滤波是用观测量去修正估计量
2.在x(k) = x_(k) + K( y(k) - H*x_(k) )中，H*x_(k)为由当前时刻估计值通过观测模型矩阵估计出来当前时刻的观测量，而y(k)是此刻的实际观测值
3.P_(k) = A * P(k-1) * A^T + Q计算的是：估计值与真实值之间的误差的协方差，并且这个协方差也是一个先验估计。而后面的P(k)= (I- K * H ) P_(k)相当于矫正这个协方差。
4.推荐个博客：https://blog.csdn.net/czyv587/article/details/52061523 ，帮助你联系实际来理解kf

---

EKF

首先需要明白，ekf的出现是因为控制系统不可能永远都是线性系统，存在非线性系统，而针对非线性系统，上面出现的公式就需要进行修改，修改的方式是：

1.原先的x_(k) = A*x(k-1) + B*u_(k-1) 改为x_(k) ＝ f（x(k－1）,u_(k-1)),f第一个非线性函数。
2.原先的P_(k) = A * P(k-1) * A^T + Q 中的A矩阵要改变，变为：将非线性系统中的f对x求偏导（x(k-1)处）得到新的A
3.原先的 x(k) = x_(k) + K( y(k) - H*x_(k) )中的H换成：h对x求偏导（x_(k)处）得到新的H，h也是非线性函数
4.K = P_(k)* H^T * ( H * P_(k) * H^T + R) ^(-1)中的H也要换成上面新求得H
5.P_(k)= (I- K * H ) P_(k)中的H也要换成上面新求得H。

6.注意，f对x求偏导得到的矩阵称为f的雅克比矩阵，h对x求偏导得到的矩阵称为观测雅克比矩阵
雅各比矩阵就是方程矩阵对每一个变量的偏导数。
则在向量分析中，雅可比矩阵Jacobian matrix是该函数的所有分量（ m 个）对向量 x 的所有分量（ n 个）的一阶偏导数组成的矩阵。