卡尔曼滤波原理

以最简单的，单输入，单输出系统为例

这个视频《理解卡尔曼滤波器 - Understanding Kalman Filters》讲的比较好，

视频链接https://www.bilibili.com/video/av24098897/?p=3

问题分析：很显然，我们可以通过对速度v的积分来获取当前位置，可惜这个值s1还有累计误差，而且当小车打滑、过沟时，我们手里的v的值也就并不是那么准确，这样得出的预测值s1，就不是很可靠。这一过程我们称为：用系统的数学模型来预估。

还有一个获得路程s的方法就是读取GPS的值，减掉小车的起始位置，为了叙述简便，我们直接把这两者的差值叫做：GPS的输出值，由于GPS的输出值是有噪声的，GPS读出的测量值S2，也不是很可靠。这一过程我们称为：用测量值来预估。

控制器的计算过程只能是离散的，无法使用积分，只能使用累加：假设我们每20ms计算一次，那么预测值：

s1(k)=s1(k-1) + Bv + w

式中，B为采样时间0.02s，v为小车的速度m/s，w为误差，一般假设该误差符合正态分布N(0, Q)，0为均值，Q为方差

另一个预测值我们直接从GPS读出：

s2(k)=sg + z

式中，sg为GPS读出的值，z为GPS的噪声引起的误差，一般假设该误差符合正态分布N(0, R)，0为均值，R为方差

注意：由于s1的累计误差问题，实际上，我们递推s1(k)时，并不是用的直接用的s1(k-1)，而是用卡尔曼最优的估计值。

卡尔曼滤波器公式：

sk(k)= sk(k-1)+Bv + k*(sg - sk(k-1))

式中，sk(k)为第k次卡尔曼的估计值，sg为GPS读出的值，k为系数，显然这里k=1

前两项称为：预估器（在读GPS之前的预估值），第三相称为：更新值

在了解卡尔曼滤波器之前，先得来了解一下“状态观测器”。场景是这样的：在t时刻给火箭提供的燃料量为w(t)，火箭喷嘴的温度为Tin，为了了解Tin(t)与w(t)的关系，我们必须要获取很多组样本(w1,Tin1)、(w2,Tin2)······，可惜喷嘴的温度太高，我们无法测量喷嘴的温度，取而代之，我们只能测量喷嘴外部的外壳的温度Text

  

上图（左）中，上面为真正的火箭，下面为它的数学模型，因为没有直接馈通，该模型不含D矩阵，当然，这个数学模型我们无法使它精确的描述火箭，暂且用一个一阶系统来描述，即右图。我们的目的是，通过输入量u，实时预测喷嘴真正的温度x，我们能拿到的数字量有：输入量u，实测的外壳温度y，根据这两个值，我们把u代入数学模型表达式（上面右图的下面的框图），可以得到用数学模型预测的外壳温度值和喷嘴温度，那么这个预测值能不能当做喷嘴的真实温度x呢？要回答这个问题，我们必须先研究一下这两者的差值：ex

令K=Bk，两式相减得：

解这个一阶线性微分方程，得：，其中ex0为ex在0时刻的初值

显然，当A-KC<0时，预测的喷嘴温度与真实的喷嘴温度x的差值，会随着时间的延长，逐渐变为0，这时，我们就可以用来当做x的最优预测值了。显然，增大K值，可以使e更快的变为0。

下面进入卡尔曼滤波器：

情景假设：我们有一辆小车，我们可以用遥控器任意调节小车的速度v，小车上安装了一个GPS，任务要求：让小车从一个位置沿直线出发，然后实时返回已走过的路径长度s。

小车实际的状态空间模型如上图所示，这个表达式是用离散状态空间表达式。w和v分别代表了系统误差和测量误差，一般假设两者都满足正态分布：w~N(0，Q)，v~N(0, R)。Q代表了用数学模型来预测真实系统的偏离的程度。

上图中，上半部为实际的小车，下半部为我们提出的数学预测模型，这样就跟前文的火箭类似了，这里我们能通过模型给出一个预测的位置值，也能通过GPS获得一个测量值y。

真实的小车模型中带有系统误差，例如车轮打滑，或者车轮进坑等，使得路程不完全等于速度的积分。这里相比火箭问题更糟糕的情况是，测量值也有很大误差，火箭问题中，我们就把测量的外壳温度当做外壳的真实温度了，但在这里，GPS的测量路程，我们还不能完全当做真实的路程，它也有误差.

数学模型给出的预测值为：

式中，为预测值，u为输入值，yk为GPS的测量值，k是反馈系数。这个式子就是我们的状态观测器。

上式的前两项称为预估器，第三项称为更新值，把上式简化一下，变为：

实际上，我们在计算预估值，和更新值时，还得顺便计算一下新的系统误差的协方差和测量误差的协方差，也即：