信息处理 - 用于 3D 位置的朴素卡尔曼滤波器 - 吾爱随笔录

用于 3D 位置的朴素卡尔曼滤波器

信息处理卡尔曼滤波器

2022-01-31 19:38:11

我查看了讨论 3D 卡尔曼滤波器的帖子。卡尔曼滤波器估计节点的 3D 位置帮助卡尔曼滤波器实现估计 3D 位置

据我了解，两人都在尝试相同的事情，即根据加速器读数计算 3D 位置。然而，他们设置卡尔曼滤波器的过程完全不同。

我自己的方法也不一样

这些是基于我在这里学到的公式，它将加速度视为U向量http://www.cl.cam.ac.uk/~rmf25/papers/Understanding%20the%20Basis%20of%20the%20Kalman%20Filter .pdf

Xk = AXk-1 + BUt
x_curr = x_prev + x_vel_prev * dt + 0.5*dt^2*x_acc
x_vel_curr = x_vel_prev + dt*x_acc

第二篇文章似乎完全类似于这个例子http://campar.in.tum.de/Chair/KalmanFilter#Literature删除了U向量。

虽然第一篇文章有一个基于 wiki 的不同过程方程，但它使用Ga而不是Bu https://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical：

X = FX_prev + Ga

这就是我所做的，我认为这是有道理的：

X = [x,y,z,dx,dy,dz] 

U = [ddx, ddy, ddz]

A = np.matrix([[1, 0, 0, dt, 0,  0],
               [0, 1, 0, 0,  dt, 0],
               [0, 0, 1, 0,  0,  dt],
               [0, 0, 0, 1,  0,  0],
               [0, 0, 0, 0,  1,  0],
               [0, 0, 0, 0,  0,  1]],
             )
B = np.matrix([[0.5*(dt**2), 0,           0],
               [0,           0.5*(dt**2), 0],
               [0,           0,           0.5*(dt**2)],
               [dt,          0,           0],
               [0,           dt,          0],
               [0,           0,           dt]])

H = np.matrix([[1,0,0,0,0,0],
              [0,1,0,0,0,0],
              [0,0,1,0,0,0]])
P = np.matrix([[10,0,0,0,0,0],
              [0,10,0,0,0,0],
              [0,0,10,0,0,0],
              [0,0,0,100,0,0],
              [0,0,0,0,100,0],
              [0,0,0,0,0,100]])

Q = np.matrix([[1,0,0,0,0,0],
              [0,1,0,0,0,0],
              [0,0,1,0,0,0],
              [0,0,0,1,0,0],
              [0,0,0,0,1,0],
              [0,0,0,0,0,1]])

R = np.matrix([[1,0,0],
              [0,1,0],
              [0,0,1]])

我不明白的是，前两个帖子怎么有完全不同的矩阵？有什么原因吗？我的方法正确吗？

非常感谢任何输入和建议

1个回答

你应该记住的是，R Kalman 在 1962 年（或大约在那时）发表了他的开创性论文。他是一名控制工程师，在广泛使用的高保真数值多物理场建模之前对真实系统感兴趣。您对比的运动学模型源自通常是一个常数点质量。飞机和火箭会消耗质量进行推进，因此我们可以很早就看到会有被忽略的术语。还应包括气动阻力，对于层流，阻力大致成比例

F = m a = m \frac{d v}{d t} .

$F=ma =m \frac{dv}{dt}.$

m

$m$

F

$F$

v^{2}

$v^2$ 这甚至不接近 Navier-Stokes 方程。因此，即使是简单的阻力模型也是非线性的，对于实时在线算法来说，线性状态空间模型就是他真正拥有的一切。这些过度简化的解决方案是噪声和小步长以及预测反馈。

噪声也可以是实际的随机现象。所以很多非平凡的建模错误集中在其中可以是控制力、随机力或我们视为噪声的伪建模错误。我们还有一组测量方程，其中还可以包含真实或概念的噪声。您可以坚持使用这种形式并使用卡尔曼-布西滤波器，或者您可以将连续状态空间模型转换为差分方程，该方程通常用于卡尔曼滤波器。为此，您需要整合刚刚给出的方程。我不打算详细介绍，因为有书籍和网站展示了这一点，但首先要计算矩阵指数

\frac{d}{d t} x (t) = A x (t) + B u (t)

$\frac{d}{dt} \mathbf{x}(t)= \mathbf{A}\mathbf{x}(t) + \mathbf{B} \mathbf{u}(t)$

u (t)

$\mathbf{u}(t)$

F = e^{A Δ t}

$\mathbf{F}= \mathbf{e}^{\mathbf{A} \Delta t}$ 。您的一些示例计算矩阵指数，而有些则不计算。您通常可以获得矩阵指数的符号解，或者您可以数值求解。

回到你原来的问题是这些模型是简化的简化。可能会做出糟糕的选择，但实际上并没有一个正确的离散时间 3D 状态空间模型。有些可能有有些可能包括。真实物体不是点质量。角度方向很重要。许多缺失的内容在小时间步长上被视为噪声。当非线性更严重时，我们切换到扩展卡尔曼滤波器。未来，状态模型将是实时的多物理场数值程序。 $x_{k+1}=x_{k} + \Delta t v_{k}$ $\frac{(\Delta t)^2}{2} a_k$

最好的模型是最适合您的数据的模型。

其它你可能感兴趣的问题

上一篇音频/音乐均衡器中“动态可重新计算”滤波器的数学特性的介绍性资源？下一篇互相关，0 处的尖峰？