什么是卡尔曼滤波？
为什么要估计事物的状态？因为我们不能准确地知道物体的当前状态，所以我们需要测量来获得物体的状态。但测量值不准确，总是有噪音。卡尔曼滤波器是一种结合预测（先验分布）和测量更新（似乎）的状态估计算法。 预测模块是为物体的运动建立运动模型。据估计，栗子中的行人状态采用恒速模型（CV），运动模型通过转换上一刻的最佳估计，得到当前时刻的估计状态和预测误差。 根据当前时刻的测量值和预测状态，对当前时刻的状态进行最优估计。
2.卡尔曼公式
Predict： X k = F X ^ ? k ? 1 B U k w k { {X}_{k}}=F{ {\overset{\hat{\ }}{\mathop{X}}\,}_{k-1}} B{ {U}_{k}} { {w}_{k}} Xk=FX^k?1 BUk+wk P k = F P k − 1 F + Q k { {P}_{k}}=F{ {P}_{k-1}}F+{ {Q}_{k}} Pk=FPk−1F+Qk 其中 X k { {X}_{k}} Xk为k时刻的状态预测值，F为状态转移矩阵， X ^ k − 1 { {\overset{\hat{\ }}{\mathop{X}}\,}_{k-1}} X ^k−1为k-1时刻的最优估计。 B U k B{ {U}_{k}} BUk为外部输入， w k { {w}_{k}} wk为过程激励噪声。P为预测误差， P k { {P}_{k}} Pk表示为k次先验估计协方差矩阵， P k − 1 { {P}_{k-1}} Pk−1为k-1次后验估计协方差矩阵。 Q k { {Q}_{k}} Qk为过程激励噪声协方差矩阵。 Update： K K = P K H T ( H P K H T + R ) − 1 { {K}_{K}}={ {P}_{K}}{ {H}^{T}}{ {(H{ {P}_{K}}{ {H}^{T}}+R)}^{-1}} KK=PKHT(HPKHT+R)−1 X K ^ = X K + K K ( Z K − H X K ) \overset{\hat{\ }}{\mathop{ { {X}_{K}}}}\,={ {X}_{K}}+{ {K}_{K}}({ {Z}_{K}}-H{ {X}_{K}}) XK ^=XK+KK(ZK−HXK) P K = ( 1 − K K H ) P K { {P}_{K}}=(1-{ {K}_{K}}H){ {P}_{K}} PK=(1−KKH)PK 其中K为卡尔曼增益，H为观测矩阵，R为测量噪声协方差矩阵， Z K { {Z}_{K}} ZK为k时刻测量值。
3.以行人状态估计举个栗子
①predict:
先对行人状态建立一个运动模型，我们在此选择恒速度运动模型， X = ( p x , p y , v x , v y ) T X={ {({ {p}_{x}},{ {p}_{y}},{ {v}_{x}},{ {v}_{y}})}^{T}} X=(px,py,vx,vy)T,此时 B U k B{ {U}_{k}} BUk为0，过程噪声 w k { {w}_{k}} wk其实是行人加减速引起的。公式如下： X K = [ 1 0 △ t 0 0 1 0 △ t 0 0 1 0 0 0 0 1 ] [ p x p y v x v y ] k − 1 + [ 1 2 a x △ t 2 1 2 a y △ t 2 a x △ t a y △ t ] k − 1 { {X}_{K}}=\left[ \begin{matrix} 1 & 0 & \vartriangle t & 0 \\ 0 & 1 & 0 & \vartriangle t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right]{ {\left[ \begin{matrix} { {p}_{x}} \\ { {p}_{y}} \\ { {v}_{x}} \\ { {v}_{y}} \\ \end{matrix} \right]}_{k-1}}+{ {\left[ \begin{matrix} \tfrac{1}{2}{ {a}_{x}}\vartriangle { {t}^{2}} \\ \tfrac{1}{2}{ {a}_{y}}\vartriangle { {t}^{2}} \\ { {a}_{x}}\vartriangle t \\ { {a}_{y}}\vartriangle t \\ \end{matrix} \right]}_{k-1}} XK=⎣⎢⎢⎡10000100△t0100△t01⎦⎥⎥⎤⎣⎢⎢⎡pxpyvxvy⎦⎥⎥⎤k−1+⎣⎢⎢⎡21ax△t221ay△t2ax△tay△t⎦⎥⎥⎤k−1