有一个关于实现一维卡尔曼滤波器的作业。

目标是让蓝色方块击中粉红色方块。

蓝色方块获取关于粉红色方块位置的数据，但数据不够精确，存在噪声。噪声数据的标准偏差为300像素。

蓝色方块使用卡尔曼滤波算法来获取精确的位置并击中粉红色方块。

只允许编辑”Kalman类”，我已经在那个类中实现了卡尔曼滤波算法。

我使用了描述的5个方程，但它仍然无法正常工作。

这是实现卡尔曼滤波器的Kalman类（下面是整个代码）：

class Kalman():    def __init__(self):        self._dt = 1  # 时间步长        self._pos = 785 # 位置        self._vel = 1 # 速度        self._acc = 0.05 # 加速度        self._pos_est_err = 1000000 # 位置估计不确定性        self._vel_est_err = 1000000 # 速度估计不确定性        self._acc_est_err = 2000 # 加速度估计不确定性        self._pos_mea_err = 300        self._vel_mea_err = 100        self._acc_mea_err = 4        self._alpha = 1 # 收敛到0.32819526927045667        self._beta = 1 # 收敛到0.18099751242241782        self._gamma = 0.01 # 加速度变化不大                self._q = 30 # 位置过程不确定性        self._q_v = 4 # 速度过程不确定性        self._q_a = 2 # 加速度过程不确定性                self._last_pos = 785        self._measured_velocity = 0    def calc_next(self, zi):                self._measured_velocity = zi - self._last_pos        self._last_pos = zi                # 状态外推        self._pos = self._pos + self._vel*self._dt + 0.5*self._acc*self._dt**2        self._vel = self._vel + self._acc*self._dt        self._acc = self._acc                # 协方差外推        self._pos_est_err = self._pos_est_err + self._dt**2 * self._vel_est_err + self._q        self._vel_est_err = self._vel_est_err + self._q_v        self._acc_est_err = self._acc_est_err + self._q_a                # 阿尔法-贝塔-伽马更新        self._alpha = self._pos_est_err / (self._pos_est_err + self._pos_mea_err)        self._beta = self._vel_est_err / (self._vel_est_err + self._vel_mea_err)        #self._gamma = self._acc_est_err / (self._acc_est_err + self._acc_mea_err)                # 状态更新        pos_prev = self._pos        self._pos = pos_prev + self._alpha*(zi - pos_prev)        self._vel = self._vel + self._beta*((zi - pos_prev)/self._dt)        self._acc = self._acc + self._gamma*((zi - pos_prev) / (0.5*self._dt**2))                # 协方差更新        self._pos_est_err = (1-self._alpha)*self._pos_est_err        self._vel_est_err = (1-self._beta)*self._vel_est_err        self._acc_est_err = (1-self._gamma)*self._acc_est_err                return self._pos

我使用了”阿尔法-贝塔和伽马”值作为卡尔曼增益，因为对象（粉红色方块）在移动并改变方向。

但是，我该如何编写协方差外推方程和协方差更新方程呢？

我不确定我是否正确地编写了它们。

完整代码

完整代码保持不变

回答：

你的协方差更新看起来是正确的，但位置估计的公式存在错误。考虑到你的过程模型是线性变换 x_k = F_k x_k-1（忽略噪声项），其中 x_k 是包含 self._pos、self._vel 和 self._acc 的列向量，F_k 是以下矩阵：

1  ∆t  ½∆t²0   1   ∆t0   0   1

其中 ∆t 是代码中的 self._dt。更新的协方差矩阵 P_k|k-1 的公式，在你的情况下是一个对角矩阵，是 P_k|k-1 = F_k P_k-1|k-1 F_k^T + R_k，其中 P_k-1|k-1 是前一个协方差矩阵，R_k 是反映测量误差的对角协方差矩阵（即 self._pos_mea_err、self._vel_mea_err 和 self._acc_mea_err）。当你进行矩阵乘法时，F_k 中的所有 ∆t 项因为不在对角线上而被清零。因此，你的协方差外推中不应该有 ∆t 项。