卡尔曼滤波器实例:进一步讨论自由落体运动

上一篇中,我们使用向量[0 0 0]T作为系统的初始状态,并且使用$infty$作为初始状态误差的方差,也就是假定物体的初始位置、初始速度、加速度都是未知的,所有的状态信息都通过Radar的测量结果估计出来。但实际上,物体的加速度是预先知道的(假定为标准重力加速度),因此,我们可以简化上一篇中的多项式,以得到更好的状态估计。

将物体的位置设为状态$x_1$:

$x_1=s_0-v_0t-frac{1}{2}gt^2$
$frac{dx_1}{dt}=x_2=-v_0t-gt$
$frac{dx_2}{dt}=-g$

上一篇中,我们暂时“忘记了”g的数值,因此$frac{dx_2}{dt}$仍然是一个需要估计的系统状态,需要继续微分。而此处,$frac{dx_2}{dt}$是已知的,因此我们到此就可以写状态方程了:

$left[ egin{matrix} frac{dx_1}{dt} \ frac{dx_2}{dt} end{matrix} ight] = left[  egin{matrix} 0&1 \ 0&0 end{matrix} ight] left[  egin{matrix}  x_1 \ x_2 end{matrix} ight] + left[ egin{matrix} 0\-1  end{matrix} ight]g$

将这个矩阵和如下标准状态矩阵对比:

$mathbf{dot{x}}=mathbf{Fx}+mathbf{Gu}+mathbf{w}$

可得:

$mathbf{F}=left[ egin{matrix}  0&1 \ 0&0 end{matrix} ight]$

$mathbf{G}=left[ egin{matrix} 0 \ -1 end{matrix} ight]$

$mathbf{u}=g$

将连续时间状态模型转换到离散时间:

$oldsymbol{Phi}_k=left[  egin{matrix}  1&T_s\0&1 end{matrix} ight]$

$mathbf{G}_k=int_0^{T_s}oldsymbol{Phi}( au)mathbf{G}d au=int_0^{T_s}left[ egin{matrix} 1& au \ 0&1 end{matrix} ight] left[ egin{matrix} 0\-1 end{matrix} ight]d au=left[ egin{matrix} -frac{T_s^2}{2} \ -T_s end{matrix} ight]$

原文地址:https://www.cnblogs.com/byeyear/p/6790660.html