本文主要介绍如何设计一个卡尔曼滤波模型。包含基本的数学推导。

本文采用的基本模型为CT模型。

若无特殊说明,本文的记号采用Labbe notation。

在本例中,已知目标圆周运动的角速度、运动半径。观测量为运动物体的二维坐标。状态估计量为运动物体的坐标、速度、运动中心。

滤波器设计

匀速圆周运动估计模型

这里将运动中心的估计从模型中抽离。第一阶段仅估计目标的坐标、速度。第二阶段采用一个静止模型滤波器对中心进行滤波。这样可以加快第一阶段的计算速度。并且更易于调参。

状态量

$$\rm x=\begin{bmatrix}x\\ \dot{x} \\ y\\ \dot{y}\end{bmatrix}$$

状态转移矩阵

这里采用CT模型的转移矩阵设计。

$$\rm F= \begin{bmatrix} 1 & \frac{\sin(\omega\Delta t)}{\omega} & 0 & -\frac{1-\cos(\omega\Delta t)}{\omega}\\ 0 & \cos(\omega\Delta t) & 0 & -\sin(\omega\Delta t) \\ 0 & \frac{1-\cos(\omega\Delta t)}{\omega} &1 & \frac{\sin(\omega\Delta t)}{\omega}\\ 0 & \sin(\omega\Delta t) & 0 & \cos(\omega\Delta t) \end{bmatrix}$$

该转移方程与中心、半径无关。其仅利用 $\omega$ 这一个已知参数,其原理是将 $\begin{bmatrix}\dot x_{k-1}\\ \dot y_{k-1}\end{bmatrix}$ 旋转 $\omega\Delta t$ 得到新的速度 $\begin{bmatrix}\dot x_{k}\\ \dot y_{k}\end{bmatrix}$、计算$\Delta t$ 时间后新的位置 $\begin{bmatrix}x_k\\ y_k\end{bmatrix}$。具体公式推导此处不展开。

仅利用 $\omega$ 作状态转移的好处是,跟踪目标的 $\omega$ 仅仅与时间有关。它往往不会因为解算而出现较大误差。如果引入半径,PnP解算过程存在的误差可能使得模型发散。此处待进一步验证。

过程噪声

1. 连续模型

$$\rm Q = \int_0^{\Delta t} \rm F(t) \rm Q_c F^\top(t)\rm d t$$

其中 $\rm Q_c$ 为连续噪声(continuous noise)。

我们认为,状态估计的噪声来自于两个速度的估计值,如下设置连续噪声,其中的 $\Phi_s$ 可以理解为状态转移的“谱密度(spectral density)”。

$$\rm Q_c= \begin{bmatrix} 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\Phi_s$$

计算 $\rm Q$ 将所得结果泰勒展开至对应位置的最低阶。

借用sympy库进行计算:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

import sympy

from sympy import *

init_printing(use_latex='mathjax')
dt, phi, a = symbols('\Delta{t} \Phi_s \omega')

# F_k=Matrix(
#     [[1, sin(a*dt)/a, 0 , -(1-cos(a*dt)/a)],
#     [0, cos(a*dt), 0, -sin(a*dt)],
#     [0, (1-cos(a*dt))/a, 1, sin(a*dt)/a],
#     [0, sin(a*dt), 0, cos(a*dt)]]
# )

# 为了简化结果,此处移除了角速度的参量。

F_k=Matrix(
    [[1, sin(dt), 0 , -(1-cos(dt))],
    [0, cos(dt), 0, -sin(dt)],
    [0, (1-cos(dt)), 1, sin(dt)],
    [0, sin(dt), 0, cos(dt)]]
)

Q_c = Matrix(
    [[0, 0, 0, 0],
    [0, 1, 0, 0],
    [0, 0, 0, 0],
    [0, 0, 0, 1]] 
)* phi

Q = integrate(F_k * Q_c * F_k.T, (dt, 0, dt))


Q_expanded = Matrix.zeros(Q.rows, Q.cols)

# 循环处理矩阵中的每个元素
for i in range(Q.rows):
    for j in range(Q.cols):
        # 获取当前元素
        element = Q[i, j]
        # 使用 sympy.series 方法展开元素,并找到最低阶
        taylor_series = series(element, dt, 0)
  
        # 提取展开后的最低阶
        lowest_order_term = taylor_series.as_ordered_terms()[0]
    
        # 将最低阶的项赋给新矩阵
        Q_expanded[i, j] = lowest_order_term

Q_expanded = Q_expanded/phi
# 化简
Q_expanded = simplify(Q_expanded)

print(latex(Q_expanded))

最终结果为

$$\rm Q=\left[\begin{matrix}\frac{\Delta{t}^{3}}{3} & \frac{\Delta{t}^{2}}{2} & 0 & \frac{\Delta{t}^{3}}{6}\\\frac{\Delta{t}^{2}}{2} & \Delta{t} & - \frac{\Delta{t}^{3}}{6} & 0\\0 & - \frac{\Delta{t}^{3}}{6} & \frac{\Delta{t}^{3}}{3} & \frac{\Delta{t}^{2}}{2}\\\frac{\Delta{t}^{3}}{6} & 0 & \frac{\Delta{t}^{2}}{2} & \Delta{t}\end{matrix}\right] \Phi_{s}$$

一些论文结果会将负项移除。

2. 离散模型

参照Kalman-Filter-Math。进行离散化积分。

$$\rm Q = \mathbb E[\Gamma w(t) w(t) \Gamma^\top] = \Gamma\sigma^2_v\Gamma^\top$$

其中

$$\Gamma = \begin{bmatrix} \frac{\Delta T^2}{2} & 0\\ T & 0\\ 0 & \frac{\Delta T^2}{2}\\ 0 & T \end{bmatrix}$$

此处计算可以同上采用sympy计算:

$$\left[\begin{matrix}\frac{\Delta{t}^{4}}{4} & \frac{\Delta{t}^{3}}{2} & 0 & 0\\\frac{\Delta{t}^{3}}{2} & \Delta{t}^{2} & 0 & 0\\0 & 0 & \frac{\Delta{t}^{4}}{4} & \frac{\Delta{t}^{3}}{2}\\0 & 0 & \frac{\Delta{t}^{3}}{2} & \Delta{t}^{2}\end{matrix}\right] \sigma^{2}_{v}$$

理论上来说第一种更为精确。

观测噪声

可以根据观测目标的距离设定不同的观测噪声。

此处需要具体情况具体分析。

中心估计模型

由于已知旋转的半径。可以利用当前已知的速度方向和位置。从当前位置沿与速度垂直的方向移动半径长度即可得到中心点的位置。

将所得的中心点位置送入一个静止模型的滤波器中,即可对中心进行估计。在滤波器刚启动时,中心的位置估计往往不准确,可以随着时间梯度下降该滤波器的过程噪声。

滤波器的代码实现(C++)

可以参照https://github.com/MicDZ/Filters。

该仓库提供了一个模拟器和一个滤波器。