方程的数量等于状态变量的数量
你的方程描述了如何从时间 t-1
的值计算出时间 t
的变量 x
和 y
(我稍作修正):
x(t) = x(t-1) + vx*dt
y(t) = y(t-1) + vy*dt
dt
是“时期”t-1
和t
之间的时间(后者是时间指数,而不是实际时间)
我们可以看到计算涉及到不仅x
和y
还有vx
和vy
,根据牛顿运动方程(p = p0 + v*t
)。所以你的系统有4个变量,其中2个是被观察到的(通过对x
和y
的测量),另外2个是隐藏的(它们没有测量但是由滤波器计算得出)。这些变量构成了系统的“状态”X
:
X = [x, y, vx, vy]
您提供了两个方程,还有两个缺失的方程,这些方程与速度有关。但是在这里假设的预测模型是一个恒定速度运动,因此状态中的速度不会改变,例如
vy
的预测方程只是:
vy = vy
注意:
预测和校正阶段
滤波器工作分为两个阶段:预测阶段,然后是校正阶段,名称可能有所不同。您的问题与预测相关。
预测是基于您提供给滤波器的模型来预测时刻t的状态,该模型以方程的形式表示,每个变量对应一个方程。每个方程都是涉及所有变量的项的总和(原始卡尔曼滤波器的线性方程)。
校正(更新)是通过使用输入中提供的实际测量z,按比例调整预测结果,这个比例系数K由滤波器在每个预测/校正周期中计算和更新。K(0到1)表示滤波器对自己的预测(K=0)或测量(K=1),或两者的信任程度,比例为1-K和K。这里的信任程度由相关的高斯方差来量化。
构建预测(转移)矩阵F
如果我们看一下预测阶段,例如对于y,我们必须提供以下类型的方程:
y(t) = f1.x(t-1) + f2.y(t-1) + f3.vx(t-1) + f4.vy(t-1)
现在这个带有指数的方程是数学家用于证明的方程,但对于一个IT工程师来说,由于t和t-1总是隐含的,分别位于等号左边和右边,所以可以简化为:
y = f1.x + f2.y + f3.vx + f4.vy
根据你的方程式
y
:
y = y + vy*dt
= 0.x + 1.y + 0.vx + dt.vy
一种可能的简化方法是使用矩阵同时执行多个操作。
[y] = [f1, f2, f3, f4] . ⎡x ⎤
⎢y ⎥
⎢vx⎥
⎣vy⎦
你将最后的矩阵识别为垂直编写的状态向量,而中间的矩阵是所谓的"过渡矩阵"F
。如前所述,过渡矩阵中的系数顺序必须与您选择的状态顺序相匹配。
对于那些忘记了矩阵相乘(点乘)的人,可以在这里查看。
Kalman滤波器不一定要使用矩阵,它们也可以使用单独的方程。但是,在短时间的熟悉后,使用矩阵会更简单,因为它们可以同时处理多个变量,并具有巨大的优势:基于矩阵运算的公式在变量数量发生变化时仍然有效。
在这里可以看到这一点:我们为y
所做的操作也可以用来计算其他3个变量。同样的过渡矩阵F
可以使用,每个方程占据一行,结果将是一个包含4个变量的状态向量(在时间t
):
⎡x ⎤ = ⎡?, ?, ?, ? ⎤ . ⎡x ⎤
⎢y ⎥ ⎢f1, f2, f3, f4⎥ ⎢y ⎥
⎢vx⎥ ⎢?, ?, ?, ? ⎥ ⎢vx⎥
⎣vy⎦ ⎣?, ?, ?, ? ⎦ ⎣vy⎦
所以让我们用从这四个方程中所知的所有系数来写出矩阵 F:
⎡x ⎤ = ⎡1, 0, dt, 0 ⎤ . ⎡x ⎤
⎢y ⎥ ⎢0, 1, 0, dt⎥ ⎢y ⎥
⎢vx⎥ ⎢0, 0, 1, 0 ⎥ ⎢vx⎥
⎣vy⎦ ⎣0, 0, 0, 1 ⎦ ⎣vy⎦
要清楚地阅读这个问题:x
在t
时刻(左侧状态向量的第一个变量)等于系数为1, 0, dt, 0
(F矩阵的第一行)的项之和,而变量分别是在t-1
时刻(右侧向量)的状态向量中的x, y, vx, vy
重要注意事项
用于预测的模型是一个恒定速度运动模型。通常不需要使用滤波器,因为位置可以准确地表示为p = p0 + v * 常数
,在绘图时呈直线。我们使用滤波器是因为我们知道系统与该模型有些出入。但如果模型不准确,那么为什么要使用它呢?
滤波器能够处理具有恒定速度模型的缓慢变化的速度情况,但在预测和修正阶段都需要引入不确定性阈值。
这种不确定性实际上是一个方差/协方差矩阵。方差和协方差告诉滤波器使用系数K
来调整其校正阶段,以便在自身预测(矩阵Q
)和测量值(矩阵R
)方面不要过于自信。提供这些矩阵意味着您有一些方法来预估方差,否则我们将需要通过一些测试来调整这些矩阵,这可能会很繁琐。
示例
您会注意到我在代码中没有使用矩阵,而是使用了简单的Numpy数组。原因是这样更好,矩阵没有额外的优势,反而带来了不便之处(参见deprecation)。Numpy中的点积运算使用语法糖数组操作符@
进行简化。
def predict(x, P):
x = F @ x
P = F @ P @ F.T + Q
return x, P
def update(x, P, z):
S = H @ P @ H.T + R
K = P @ H.T @ np.linalg.inv(S)
y = z - H @ x
x = x + K @ y
P = P - K @ H @ P
return x, P
dt = 1
Q_std = 0.04
R_std = 0.35
F = np.array([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
q = np.array([[0.0004, 0.0008],
[0.0008, 0.0016]])
Q = la.block_diag(q, q)
H = np.array([[1, 0, 0, 0],
[0, 0, 1, 0]])
R = np.eye(2) * R_std**2
x = np.array([[0, 0, 0, 0]]).T
P = np.eye(4) * 500
for z in zs:
x, P = update(*predict(x, P), z)
![enter image description here](https://istack.dev59.com/HzK1d.webp)