如何为 3d 对象跟踪创建卡尔曼滤波器

Question

我需要在 python 中为 3d 对象跟踪创建卡尔曼滤波器 我不明白我应该如何创建这些矩阵以及从哪里进行测量 如果有任何 3d 对象的 KD 示例请与我分享

我已经看到用于 2d 对象跟踪的 KD 但是已经给出了所有矩阵 我唯一要做的就是编写预测和更新函数

A = np.array([[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]])

# Noise covariance matrix
Q = q**2 * np.array([[dt**3/3, 0, 0, dt**2/2, 0, 0],
                     [0, dt**3/3, 0, 0, dt**2/2, 0],
                     [0, 0, dt**3/3, 0, 0, dt**2/2],
                     [dt**2/2, 0, 0, dt, 0, 0],
                     [0, dt**2/2, 0, 0, dt, 0],
                     [0, 0, dt**2/2, 0, 0, dt]])

# Measurement matrix
H = np.array([[1., 0, 0, 0, 0, 0],            
              [0., 1, 0, 0, 0, 0],
              [0., 0, 1, 0, 0, 0]])
# Measurement noise covariance matrix
R = 5 * np.eye(3)

这就是我的矩阵现在的样子 但我认为矩阵 Q 不正确 我不明白我应该如何创建它

Answer 1

不管你有多少维度。卡尔曼滤波器不是关于位置，而是关于如何预测系统状态的一般解决方案。

一般来说，您需要具有状态转换矩阵，在移动的情况下通常会添加速度列 - 每个维度一个 - 因为移动对象时每次迭代都会添加一定数量的位置。

您还需要定义状态向量元素协方差的协方差矩阵。

还有添加一定数量外部输入（如噪声、控制等）的不同矩阵。

然而，在最简单的情况下，您不需要设置所有这些。下面是借助

filterpy

包的卡尔曼滤波器实现的 3D 案例的简单示例。

import numpy as np
from filterpy.kalman import KalmanFilter

f = KalmanFilter(6, 3)

f.x = np.array([0., 0., 0., 0., 0., 0.])
f.F = np.asarray(
    [
        [1., 0., 0., 1., 0., 0.],
        [0., 1., 0., 0., 1., 0.],
        [0., 0., 1., 0., 0., 1.],
        [0., 0., 0., 1., 0., 0.],
        [0., 0., 0., 0., 1., 0.],
        [0., 0., 0., 0., 0., 1.]
    ]
)

f.H = np.array([
    [1., 0., 0., 0., 0., 0.],
    [0., 1., 0., 0., 0., 0.],
    [0., 0., 1., 0., 0., 0.]
])

def main():
    for i in range(10):
        z = np.array([i, 0.1*i, 0.01*i])
        print('Measured: ', z)
        f.predict()
        print('predicted: ', f.x)
        f.update(z)

if __name__ == '__main__':
    main()

在哪里

• f.x

  是

  [x, y, z, vx, vy, vz]

  的初始状态向量，其中

  v

  表示 velocity.

• f.F

  是您的状态转换矩阵，即描述您期望对象位置改变方式的模型。例如，根据示例中的模型，您的

  x

  将随着

  1*x + 0*y + 0*z + 1*vx + 0*vy + 0*vz

  =

  x + vx

  每一步而变化。

• f.H

  - 定义测量函数的矩阵

上面的例子使用了默认协方差矩阵，默认噪声等

P.S. - 关于

from where take the measurements

，我们无法给您任何建议。这取决于您拥有的数据或传感器。