卡尔曼滤波入门
基本概念
卡尔曼滤波中，会涉及到真值、测量误差，预测误差等概念。真值描述的是客观世界上，物体具有的状态值，这个值通过仪器测量以后，会在真值范围内呈现正态分布，测量值和真值的误差就是测量误差。
真值空间： 通常指状态变量所在的空间。有可以直接测量的，可能也包含不可直接测量的变量。
测量空间：指可直接测量的状态变量所组成的空间。
预测空间：通常和真值空间等大，用于预测状态变量的下一时刻状态。
滤波算法
一般情况下，对于一个确定系统，我们可以通过上一时刻的状态推测下一时刻的状态，比如说一辆近乎匀速的小车，它的状态变量只有位置和速度：
X t = [ s v ] X_t=\left [ \begin{matrix} s \\ v \\ \end{matrix} \right ] Xt=[sv]
在假设他匀速的情况下，下一时刻的状态为：
X t + 1 ′ = [ s + v × Δ t v ] = [ 1 Δ t 0 1 ] ∗ [ s v ] = F ∗ X t X'_{t+1}= \left [ \begin{matrix} s +v\times \Delta t \\ v \\ \end{matrix} \right ] =\left [ \begin{matrix} 1 & \Delta t \\ 0 & 1\\ \end{matrix} \right ] * \left [ \begin{matrix} s \\ v \\ \end{matrix} \right ] =F*X_t Xt+1′=[s+v×Δtv]=[10Δt1]∗[sv]=F∗Xt
由于是假设条件，所以计算得到的 X ′ X' X′是我们的预测值，或者说是估计值。
F 矩阵即我们用来做预测的状态转移矩阵，它将变量 X t X_t Xt从真值空间映射到了预测空间，得到 X t + 1 ′ X'_{t+1} Xt+1′
等到下一时刻到来时，我们在对实际状态进行测量，可以得到一个观测值 Z Z Z。观测值和预测值会产生一个误差，我们称为观测值的预测误差 Y Y Y:
Y t + 1 = Z t + 1 − H ∗ X t + 1 ′ Y_{t+1} = Z_{t+1} - H*X'_{t+1} Yt+1=Zt+1−H∗Xt+1′
在此处H可以是一个单位矩阵，H实际上是将系统状态空间映射到观测空间的映射矩阵。
假设系统状态X中存在不可直接测量的量，则通过H矩阵映射，便可将无法直接测量的量剔除，从而使状态空间映射到观测空间。
然后我们将预测误差 Y Y Y，乘以一个滤波系数 K K K，加到预测值 X ′ X' X′上，便可得到我们滤波后的状态真值 X t + 1 X_{t+1} Xt+1：
X t + 1 = X t + 1 ′ + K ∗ Y t + 1 ′ X_{t+1} = X'_{t+1}+K *Y'_{t+1} Xt+1=Xt+1′+K∗Yt+1′
当然，这里的K实际上是一个矩阵，它的作用不仅仅是设置滤波系数，更重要的是它将位于观测空间的Y映射到了状态真值空间上 。
F映射矩阵的作用是： 真值空间 => 预测空间
H映射矩阵的作用是： 真值空间 => 观测空间
K映射矩阵的作用是： 观测空间 => 真值空间
滤波系数K估计
由于观测值Z和真值也会有误差(观测误差)，它的误差范围一般由仪器决定。对于不同的状态变量，测量仪器可能不一样，所以误差自然也不一样。所以这里我们引入一个量不确定度P来衡量这种误差的大小。
不确定度越大，说明误差有可能越大
另外我们还要考虑到，不确定度可能受到外界影响而改变，所以不确定度也需要迭代式更新。
由于我们要用不确定度来计算滤波系数矩阵K，所以不确定度也得表示为一个矩阵
假设状态空间中的 位置信息的不确定度较低，速度信息不确定度更高：
P t = [ 1 0 0 20 ] P_t=\left [ \begin{matrix} 1 & 0 \\ 0 & 20 \\ \end{matrix} \right ] Pt=[10020]
我们可以根据它，通过矩阵相似变换得到预测空间的不确定度：
P t + 1 ′ = F ∗ P t ∗ F T P'_{t+1}= F * P_t *F^T Pt+1′=F∗Pt∗FT
然后根据它，再通过相似变换，得到测量空间的不确定度：
S t + 1 = H ∗ P t + 1 ′ ∗ H T S_{t+1}= H * P'_{t+1} *H^T St+1=H∗Pt+1′∗HT
由此我们便可推算滤波系数 K K K，在前面的公式中，我们知道K映射矩阵的作用是： 观测空间 => 真值空间。
所以我们得到了观测空间的测量误差后，可以乘上 S t + 1 − 1 S^{-1}_{t+1} St+1−1将其变换回观测变量的预测空间，由于观测空间和状态空间不一定一样大，所以我们再乘上 H T H^{T} HT将其映射到状态空间，最后，通过 P t + 1 ′ P'_{t+1} Pt+1′矩阵，将其从状态空间映射到了状态空间的预测空间
K = P t + 1 ′ ∗ H T ∗ S t + 1 − 1 K=P'_{t+1}*H^T*S^{-1}_{t+1} K=Pt+1′∗HT∗St+1−1
所以K矩阵的历程是 :
1、观测空间 => 观测变量的预测空间 ~~~~~~ By ~~~~~~ S t + 1 − 1 S^{-1}_{t+1} St+1−1
2、观测变量的预测空间 => 状态空间 ~~~~~~ By ~~~~~~ H T H^{T} HT
3、状态空间 => 状态空间的预测空间 ~~~~~~ By ~~~~~~ P t + 1 ′ P'_{t+1} Pt+1′
得到K矩阵后，我们前面提到不确定度P是迭代更新的，所以最后我们再更新一下P矩阵，它与状态变量的更新有异曲同工之处：
P t + 1 = P t + 1 ′ − K ∗ H ∗ P t + 1 ′ P_{t+1} = P'_{t+1} - K* H*P'_{t+1} Pt+1=Pt+1′−K∗H∗Pt+1′
式中H矩阵的作用，是将 P t + 1 ′ P'_{t+1} Pt+1′从状态空间映射到观测空间，这样我们才能和状态空间更新方程一样使用系数矩阵K。
小结
预测部分：
X t + 1 ′ = F ∗ X t + u X'_{t+1}=F*X_t+u Xt+1′=F∗Xt+u
P t + 1 ′ = F ∗ P t ∗ F T + Q P'_{t+1}=F*P_t*F^T+Q Pt+1′=F∗Pt∗FT+Q
这里的u是外部扰动（噪声），Q是过程噪声
测量与更新部分：
Y t + 1 = Z t + 1 − H ∗ X t + 1 ′ Y_{t+1} = Z_{t+1} - H*X'_{t+1} Yt+1=Zt+1−H∗Xt+1′
S t + 1 = H ∗ P t + 1 ′ ∗ H T + R S_{t+1}= H * P'_{t+1} *H^T+R St+1=H∗Pt+1′∗HT+R
K = P t + 1 ′ ∗ H T ∗ S t + 1 − 1 K=P'_{t+1}*H^T*S^{-1}_{t+1} K=Pt+1′∗HT∗St+1−1
这里的R是测量噪声矩阵
X t + 1 = X t + 1 ′ + K ∗ Y t + 1 ′ X_{t+1} = X'_{t+1}+K *Y'_{t+1} Xt+1=Xt+1′+K∗Yt+1′
P t + 1 = P t + 1 ′ − K ∗ H ∗ P t + 1 ′ P_{t+1} = P'_{t+1} - K* H*P'_{t+1} Pt+1=Pt+1′−K∗H∗Pt+1′
参考
无人驾驶技术入门（十三）| 手把手教你写卡尔曼滤波器