带加加速度的运动模型推导【Q】

啊吼！

已于 2023-06-06 15:38:53 修改

阅读量699

点赞数

分类专栏：基础概念鸡零狗碎的经验总结文章标签：算法

于 2022-12-08 09:55:10 首次发布

本文链接：https://blog.csdn.net/xujie126/article/details/128231196

版权

基础概念同时被 2 个专栏收录

15 篇文章 2 订阅

订阅专栏

鸡零狗碎的经验总结

7 篇文章 0 订阅

订阅专栏

一、前言（啰嗦）

最近工作需要，研究基于KF的运动参数滤波，对KF认知有更新，再此一并记录，如能对同道中人有所裨益，则再好不过了。

二、重要资源分享

看过很多博客，都挺好的，但是所述感性认知居多，理性分析较少，举例也大多比较简单，我暂时没有发现比较系统的KF相关网络资源（别提课本，那玩意看着犯困。），最近发现以色列一位15年工作经验的工程师的总结分享，其思路完全符合工程思维，甚好，大家好才是真的好，在此先把链接放上来哈。【建议从Example开始看】

KalmanFilter.Nethttps://www.kalmanfilter.net/multiExamples.html

内容简介：

1. 有基本的数学定理的推导总结（难能可贵的是直接和实际联系，非常实用）；

2. 有对基本数学概念的直观解释，尤其有大量的图；

3. 有对工程实践中Q/R选择、计算的详细论述；

4. 也有对不同P更新方法的解释；

2023年06月06日补充：

刚刚去查看网页，发现里面的内容被精简了，大部分关键内容放到了其售卖的电子书里了，忍痛600元购买了... ...

三、带加加速度的Q推导

上面分享的例子中，是以CA模型为例子的，因工作需要，尝试增加“加加速度”的运动模型，再此，进行推导并记录。

3.1 运动模型（恒定“加加速度”模型）

$D_{k} = D_{k-1} + v_{k-1}\Delta t + \frac{1}{2}a_{k-1}\Delta t ^{2}+ \frac{1}{4}j\Delta t^{3}$

D：距离

v：速度

a：加速度

j：(恒定)加加速度

3.2 Q推导

$\LARGE \begin{align*} V(a)&=\sigma _{a}^{2}\\ &=E(a^2)-\mu _{a}^{2}\\ &=E((j\Delta t)^2)-(\mu _j\Delta t)^2 =\Delta t^2(E(j^2)-\mu _{j}^{2})=\Delta t^2\sigma _{j}^{2} \end{align*}$

$\LARGE \begin{align*} V(v)&=\sigma _{v}^{2}\\ &=E(v^2)-\mu _{v}^{2}\\ &=E((\tfrac{1}{2}j\Delta t^2)^2)-(\tfrac{1}{2}\mu _j\Delta t^2)^2 =\tfrac{1}{4}\Delta t^4(E(j^2)-\mu _{j}^{2})=\tfrac{1}{4}\Delta t^4\sigma _{j}^{2} \end{align*}$

$\LARGE \begin{align*} V(x)&=\sigma _{x}^{2}\\ &=E(x^2)-\mu _{x}^{2}\\ &=E((\tfrac{1}{4}j\Delta t^3)^2)-(\tfrac{1}{4}\mu _j\Delta t^3)^2 =\tfrac{1}{16}\Delta t^6(E(j^2)-\mu _{j}^{2})=\tfrac{1}{16}\Delta t^6\sigma _{j}^{2} \end{align*}$ $\LARGE \begin{align*} COV(x,v)&=COV(v,x)\\ &=E(xv)-\mu _{x}\mu _{v}\\ &=E((\tfrac{1}{4}j\Delta t^3)(\tfrac{1}{2}j\Delta t^2))-((\tfrac{1}{4}\mu_j\Delta t^3)(\tfrac{1}{2}\mu_j\Delta t^2)) =\tfrac{1}{8}\Delta t^5(E(j^2)-\mu _{j}^{2})=\tfrac{1}{8}\Delta t^5\sigma _{j}^{2} \end{align*}$

$\LARGE \begin{align*} COV(x,a)&=COV(a,x)\\ &=E(xa)-\mu _{x}\mu _{a}\\ &=E((\tfrac{1}{4}j\Delta t^3)(j\Delta t))-((\tfrac{1}{4}\mu_j\Delta t^3)(\mu_j\Delta t)) =\tfrac{1}{4}\Delta t^4(E(j^2)-\mu _{j}^{2})=\tfrac{1}{4}\Delta t^4\sigma _{j}^{2} \end{align*}$

$\LARGE \begin{align*} COV(a,v)&=COV(v,a)\\ &=E(av)-\mu _{a}\mu _{v}\\ &=E((j\Delta t)(\tfrac{1}{2}j\Delta t^2))-((\mu_j\Delta t)(\tfrac{1}{2}\mu_j\Delta t^2)) =\tfrac{1}{2}\Delta t^3(E(j^2)-\mu _{j}^{2})=\tfrac{1}{2}\Delta t^3\sigma _{j}^{2} \end{align*}$

$\LARGE \begin{align*} COV(x,j)&=COV(j,x)\\ &=E(xj)-\mu _{x}\mu _{j}\\ &=E((\tfrac{1}{4}j\Delta t^3)(j))-((\tfrac{1}{4}\mu_j\Delta t^3)(j)) =\tfrac{1}{4}\Delta t^3(E(j^2)-\mu _{j}^{2})=\tfrac{1}{4}\Delta t^3\sigma _{j}^{2} \end{align*}$

$\LARGE \begin{align*} COV(v,j)&=COV(j,v)\\ &=E(vj)-\mu _{v}\mu _{j}\\ &=E((\tfrac{1}{2}j\Delta t^2)(j))-((\tfrac{1}{2}\mu_j\Delta t^2)(\mu_j)) =\tfrac{1}{2}\Delta t^2(E(j^2)-\mu _{j}^{2})=\tfrac{1}{2}\Delta t^2\sigma _{j}^{2} \end{align*}$

$\LARGE \begin{align*} COV(a,j)&=COV(j,a)\\ &=E(aj)-\mu _{a}\mu _{j}\\ &=E((j\Delta t)(j))-((\mu_j\Delta t)(\mu_j)) =\Delta t(E(j^2)-\mu _{j}^{2})=\Delta t\sigma _{j}^{2} \end{align*}$

$\large Q=\left [ \begin{matrix} \frac{\Delta t^6}{16}& \frac{\Delta t^5}{8}& \frac{\Delta t^4}{4} &\frac{\Delta t^3}{4} \\ \frac{\Delta t^5}{8}& \frac{\Delta t^4}{4}& \frac{\Delta t^3}{2} &\frac{\Delta t^2}{2} \\ \frac{\Delta t^4}{4}& \frac{\Delta t^3}{2} & \Delta t^2 &\Delta t \\ \frac{\Delta t^3}{2}& \frac{\Delta t^2}{2}& \Delta t& 1 \end{matrix} \right ]$