匀加速卡尔曼滤波模型 python代码

匀加速度卡尔曼滤波（Constant Acceleration Model）是一种常用的卡尔曼滤波模型，可以用于处理加速度变化比较明显的情况。以下是用 Python 实现的匀加速度卡尔曼滤波模型的示例代码：

import numpy as np
from numpy.linalg import inv

def kalman_filter(x, P, F, Q, H, R, z):
    # 预测
    x = np.dot(F, x)
    P = np.dot(np.dot(F, P), F.T) + Q

    # 更新
    y = z - np.dot(H, x)
    S = np.dot(np.dot(H, P), H.T) + R
    K = np.dot(np.dot(P, H.T), inv(S))
    x = x + np.dot(K, y)
    P = np.dot((np.eye(len(x)) - np.dot(K, H)), P)

    return x, P

# 初始化参数
dt = 0.1
F = np.array([[1, dt, 0.5*dt*dt], [0, 1, dt], [0, 0, 1]])
Q = np.diag([0.2, 0.1, 0.05])
H = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
R = np.diag([0.5, 0.5, 0.5])
x = np.array([0, 0, 0])
P = np.diag([0.1, 0.1, 0.1])

# 模拟数据
t = np.arange(0, 10, dt)
N = len(t)
pos = 50*t + 5*t*t
vel = 50 + 10*t
acc = 10 + np.random.randn(N)*2
z = np.vstack((pos, vel, acc)).T

# 进行卡尔曼滤波
x_filtered = np.zeros((N, 3))
P_filtered = np.zeros((N, 3, 3))
for i in range(N):
    x, P = kalman_filter(x, P, F, Q, H, R, z[i])
    x_filtered[i] = x
    P_filtered[i] = P

# 绘制结果
import matplotlib.pyplot as plt
plt.plot(t, pos, label='position')
plt.plot(t, x_filtered[:,0], label='filtered position')
plt.legend()
plt.show()

该代码使用了 `numpy` 库，通过给定模型参数和模拟数据，进行了卡尔曼滤波处理，并绘制了原始数据和滤波后的数据对比图。