KF_加速度卡尔曼滤波_python

def __init__(self, dt=0.002, q_var=0.01, r_var=0.1, p_init=1.0):
        """
         优化版 1D 位置+速度+加速度 Kalman 滤波器（固定 dt）
        :param dt: 时间步长（秒）
        :param q_var: 过程噪声方差（状态转移）
        :param r_var: 测量噪声方差
        :param p_init: 初始协方差对角值
        """
        self.dt = dt
        self.q_var = q_var
        self.r_var = r_var

        # 状态变量：x[0] = 位置, x[1] = 速度
        self.x = 0.0
        self.v = 0.0
        self.a = 0.0

        # 协方差矩阵 P（仅存储上三角或展开为变量）
        self.P00 = p_init
        self.P01 = 0.0
        self.P02 = 0.0
        self.P10 = 0.0
        self.P11 = p_init
        self.P12 = 0.0
        self.P20 = 0.0
        self.P21 = 0.0
        self.P22 = p_init

        # 过程噪声 Q（固定）
        self.Q00 = 0.0
        self.Q01 = 0.0
        self.Q02 = 0.0
        self.Q10 = 0.0
        self.Q11 = 0.0
        self.Q12 = 0.0
        self.Q20 = 0.0
        self.Q21 = 0.0
        self.Q22 = q_var

        # 测量噪声 R
        self.R = r_var

    def predict(self):
        """预测步骤（仅使用标量运算）"""
        # 状态预测：x = x + v*dt
        self.x += self.v * self.dt + 0.5*self.a* self.dt* self.dt
        self.v += self.a * self.dt

        # 协方差预测：P = F @ P @ F.T + Q
        dt = self.dt
        P00, P01, P02, P10, P11, P12, P20, P21, P22 = self.P00, self.P01, self.P02, self.P10, self.P11, self.P12, self.P20, self.P21, self.P22
        Q00, Q01, Q02, Q10, Q11, Q12, Q20, Q21, Q22 = self.Q00, self.Q01, self.Q02, self.Q10, self.Q11, self.Q12, self.Q20, self.Q21, self.Q22

        self.P00 = P00 + dt*(P10+P01) + dt*dt*(P11+0.5*P20+0.5*P02) + dt*dt*dt*(0.5*P21+0.5*P12)  + dt*dt*dt*dt*(0.25*P22) + Q00
        self.P01 = P01 + dt*(P11+P02) + dt*dt*(P12+0.5*P21) + dt*dt*dt*(0.5*P22) + Q01
        self.P02 = P02 + dt*P12 + dt*dt*(0.5*P22) + Q02
        self.P10 = P10 + dt*(P11+P20) + dt*dt*(0.5*P12+P21) + dt*dt*dt*(0.5*P22) + Q10
        self.P11 = P11 + dt*(P12 + P21) + dt*dt*P22 + Q11
        self.P12 = P12 + dt*P22 + Q12
        self.P20 = P20 + dt*P21 + dt*dt*0.5*P22 + Q20
        self.P21 = P21 + dt*P22 + Q21
        self.P22 = P22 + Q22

        # print('[predict] P_00:',self.P00,'P_11:',self.P11)
        # print('[predict] v_:',self.v,'x_:',self.x)

    def update(self, z):
        """更新步骤"""
        S = self.P00 + self.R
        if S == 0:
            return

        K0 = self.P00 / S
        K1 = self.P10 / S
        K2 = self.P20 / S  # 修正1：使用 P20

        y = z - self.x

        # 状态更新
        self.x += K0 * y
        self.v += K1 * y
        self.a += K2 * y  # 修正2：K2 大写

        # 保存旧 P
        P00, P01, P02 = self.P00, self.P01, self.P02
        P10, P11, P12 = self.P10, self.P11, self.P12
        P20, P21, P22 = self.P20, self.P21, self.P22

        # Joseph 更新
        self.P00 = P00 - K0*P00 - P00*K0 + K0*S*K0
        self.P01 = P01 - K0*P10 - P01*K0 + K0*S*K1
        self.P02 = P02 - K0*P20 - P02*K0 + K0*S*K2
        self.P10 = P10 - K1*P00 - P10*K0 + K1*S*K0
        self.P11 = P11 - K1*P10 - P11*K0 + K1*S*K1
        self.P12 = P12 - K1*P20 - P12*K0 + K1*S*K2
        self.P20 = P20 - K2*P00 - P20*K0 + K2*S*K0
        self.P21 = P21 - K2*P10 - P21*K0 + K2*S*K1
        self.P22 = P22 - K2*P20 - P22*K0 + K2*S*K2


    def get_state(self):
        return self.x, self.v