卡尔曼滤波算法原理

 一、原理

我个人认为的卡尔曼滤波算法就是：根据当前的测量值和误差加上上一刻的估计值和误差,计算得到当前的最可能的一个数值，再根据当前时刻最可能的这个数值，预测下一刻的数值. 

首先，为了形象，我们借用一个小车举一个例子：

小车有一个状态（位置和速度），我们用矩阵的形式将它表示出来就是：
$$
\vec{x}=\begin{bmatrix}
p\\
v\\
\end{bmatrix}
$$
我们对当前的状态判断含有噪声所以这个状态是符合正态分布的

（红色区域就是个正态分布）

并且因为小车的状态两个变量是相关的（位置可以由速度和时间推算），这种相关性可以用协方差矩阵来表示：
$$
Pk = \begin{bmatrix}
\Sigma pp&\Sigma pv\\
\Sigma vp&\Sigma vv\\
\end{bmatrix}
$$
协方差：用于衡量两个变量的总体误差，表示两个变量总体误差的期望，如果两个变量之间是不相关的（相互独立），则两个变量的协方差为0，反过来并不成立。

现在如果小车做匀速运动且不存在噪声误差，我们可以根据运动学公式推算出小车的位置和速度：
$$
位置：p_k=p_{k-1}+v_{k-1}*\Delta t
$$

$$
速度：v_k=v_{k-1}
$$

如果小车做加速运动（存在外部控制量）且不存在噪声误差，我们可以根据运动学公式推算出小车的位置和速度：
$$
位置：p_k=p_{k-1}+v_{k-1}*\Delta t+a*\frac{\Delta t^2}{2}
$$

$$
速度：v_k=v_{k-1}+a*\Delta t
$$

我们将上述公式写成矩阵形式
$$
匀速状态：\hat{x}_k=\begin{bmatrix}
1 & \Delta t\\
0 & 1\\
\end{bmatrix}\hat{x}_{k-1}
$$

$$
\hat{x}_k=F_k*\hat{x}_{k-1}
$$

$$
加速状态：\hat{x}_k=\begin{bmatrix}
1 & \Delta t\\
0 & 1\\
\end{bmatrix}\hat{x}_{k-1}+\begin{bmatrix}
\frac{\Delta t^2}{2}\\
\Delta t\\
\end{bmatrix}
$$

$$
\hat{x}_k=F_k*\hat{x}_{k-1}+B_k*U_k
$$

其中Fk被称为状态转移矩阵，Bk被称为控制矩阵，Uk被称为状态控制向量

同时，我们还需要更新一下我们的协方差矩阵
$$
P_k=F_k*P_{k-1}*{F_k}^T
$$
上述都是不存在噪声误差的情况下做出的理想估计，那么如果加入一个满足
$$
W_k ~~~~~~~~~N(0,Q_k)
$$
的噪声误差，我们的公式应该更新为：
$$
\hat{x}_k=F_k*\hat{x}_{k-1}+B_k*U_k+W_k
$$

$$
P_k=F_k*P_{k-1}*{F_k}^T+Q_k
$$

现在我们对小车的位置和速度做出了一个粗略的估计

我们可以对比之前的一张图我们发现我们的红色区域变宽了，这是非常合理的，因为我们初始状态的噪声误差再加上我现在估计的噪声误差导致这个正态分布的方差变大了，所以红色区域也变宽了。

光有估计值肯定是不准确的，我们的传感器测量得到的数据也不是百分之百的靠谱，因为有噪声的存在，传感器的数据也是一个正态分布，所以我们需要把传感器测量的数据和我们估计的数据结合一下，就能得出一个相对可信准确的数据了。

首先我们需要先将我们的估计值和读出的传感器数值统一成一个单位和尺度，我们将估计值数据分布按照之前的表示方法：
$$
\vec{u}_{expected}=H_k*\hat{x}_k
$$

$$
\sum{}_{expected}=H_k*P_k*{H_k}^T
$$

现在蓝色区域就是传感器返回的数值（也是正态分布），现在我们拥有两个正态分布，我们可以根据这两个正态分布得到一个新的正态分布。

红色区域为估计值
$$
（u_0,\sum{}_0）=(H_K\hat{x}_k,H_KP_k{H_k}^T)
$$
蓝色区域为测量值
$$
（u_1,\sum{}_1）=(\vec{z_k},R_k)
$$

绿色区域就是我们根据估计值和测量值结合得到的一个新的正态分布，也是我们小车最有可能存在的状态，那么问题又来了，我们同时拥有两个数值，我们怎么知道哪一个值是最可信最接近准确值的呢，换句话说就是两个正态分布，我们更应该相信哪一个，更少相信哪个一点。卡尔曼告诉了我们答案，他给出了卡尔曼增益，告诉了我们每次估计完成之后应该更相信哪个值，相信多少。
$$
卡尔曼增益：K=
\frac{P_k{H_k}^T}{H_kP_k{H_k}^T+R_k}
$$
最优估计值为
$$
\hat{x}_k'=\hat{x}_k+K(\vec{z_k}-H_k\hat{x}_k)
$$
协方差为
$$
P_k'=\frac{P_k{H_k}^T}{H_kP_k{H_k}^T+R_k}
$$