卡尔曼滤波

在卡尔曼滤波里，目标的当前状态估计考虑2个输入。

1. 一个是根据过去的状态估计当前的状态。如过去的位移为[0,3,6,9]，则当前的状态估计就应该为12。
2. 一个是当前时刻的观测状态（即传感器读数、跟踪里的检测器检测到的位置等），因此实际中观测值是带有噪声的，不是百分百精准的。

如需要估计物体的位移，则影响位移的因素有上一刻的位移，速度，外力(加速度)等。现在先忽略外力。

根据过去的状态估计当前的状态

如考虑状态向量$x$如下
$$x=\left[\begin{array}{c}位移\\ 速度\end{array}\right]$$
记$x_{t-1}$为上一刻的状态，即
$$x_{t-1}=\left[\begin{array}{c}位{移}_{t-1}\\ 速{度}_{t-1}\end{array}\right]$$

则根据上一时刻的状态，可以估计当前时刻的状态
$$\{\begin{array}{rl}位{移}_t& =位{移}_{t-1}+速{度}_{t-1}\times \Delta t\\ 速{度}_t& =速{度}_{t-1}\end{array}$$
因为忽略了外力，因此视为一个恒速运动。写为矩阵形式即为
$$x_t=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]x_{t-1}=F{x}_{t-1}\text{\hspace{1em}(1)}$$
以上的状态转移都是考虑理想状态的，实际情况中，上一刻的状态不是百分百准确的，是带有噪声的。因此其上一刻的状态实际上是个高斯分布
均值为上一刻的估计值，定义其协方差为$P_{t-1}$。又因为
$$\begin{gathered} Cov(x)=P \\ Cov(Ax)=AP{A}^T \end{gathered}$$
因此当前时刻的估计值
$$\begin{gathered} 均值x_t=F{x}_{t-1} \\ 协方差P_t=F{P}_{t-1}{F}^T+Q \end{gathered}$$
Q是因为在根据上一刻估计当前时，又引入新的噪声。
同时因为当前估计空间和观测空间中可能因为测量单位等的不同，引入测量矩阵$H$，用于将当前状态转移到观测空间。如在图像处理的跟踪里面，单位等都相同，因此可令$H=I$即单位矩阵。又或者观测值只有位移，则
$$\begin{gathered} H=\left[\begin{array}{cc}1& 0\end{array}\right] \\ {\hat{x}}_t=H{x}_t \end{gathered}$$
则${\hat{x}}_t=H{x}_t$为转移到观测空间后的估计值。 协方差为$H{P}_k{H}^T$

因此就得到了从历史状态估计的当前状态，一个均值为${\hat{x}}_t$，协方差为$H{P}_k{H}^T$的分布

当前时刻观测状态

观测值就是传感器的读数，当然也是有误差的。其分布也是高斯分布。均值即为读数(${\mu }_1$)，协方差记为R

融合高斯

至此，有了2个分布，一个是根据历史状态估计的当前状态的分布，一个是观测值
$$\begin{array}{rl}& \mathcal{N}(H{x}_t，H{P}_k{H}^T)=\mathcal{N}({\mu }_0，{\sigma }_0)\\ & \mathcal{N}({\mu }_1，R)=\mathcal{N}({\mu }_1，{\sigma }_1)\end{array}$$
因此其最终估计即为2个高斯分布的融合，融合后仍为高斯分布。
$$\begin{array}{rl}卡尔曼增益K& ={\sigma }_0({\sigma }_0+{\sigma }_1{)}^{-1}\\ 融合均值\mu & ={\mu }_0+K({\mu }_1-{\mu }_0)\\ 融合协方差\sigma & ={\sigma }_0-K{\sigma }_0\end{array}$$
融合均值即为最终的最优估计。同时将观测空间的值转换到原空间即左乘$H^{-1}$，更新后的状态用于下一时刻的迭代。