卡尔曼滤波器过滤加速度

Question

import numpy as np
from filterpy.kalman import KalmanFilter
from scipy.signal import savgol_filter
import matplotlib.pyplot as plt

# 加载数据
data = np.loadtxt('D:\新建文件夹\data.txt', delimiter='\t')

# 初始化Kalman滤波器
kf = KalmanFilter(dim_x=2, dim_z=2)
kf.F = np.array([[1., 1.], [0., 1.]])
kf.H = np.array([[1., 0.], [0., 1.]])
kf.Q = np.diag([0.1, 0.1])
kf.R = np.diag([10., 10.])
kf.x = np.array([data[0, 0], 0.])
kf.P = np.diag([10., 10.])

# 对数据进行滤波
filtered_data = []
for i in range(data.shape[0]):
    kf.predict()
    kf.update(data[i, :2])
    filtered_state = np.concatenate([kf.x, kf.P.diagonal()])
    filtered_data.append(np.concatenate([filtered_state, data[i, 2:]]))

filtered_data = np.array(filtered_data)

# 保存滤波后的数据到文件
np.savetxt('filtered_data.txt', filtered_data, delimiter='\t')

# 绘制图表
fig, axs = plt.subplots(3, 1, figsize=(10, 10))
for i in range(3):
    axs[i].plot(data[:, i], label='Raw Data')
    axs[i].plot(np.array(filtered_data)[:, i], label='Filtered Data')
    axs[i].legend()
    axs[i].set_xlabel('Time')
    axs[i].set_ylabel('Acceleration (g)')
    axs[i].set_title(f'Acceleration in {["X", "Y", "Z"][i]}  ')

plt.show()

我是想做三轴加速度的卡尔曼滤波，用chatgpt写的滤波器，然后我的z轴过滤出来的结果很奇怪，想问问这方面有了解的人是什么问题，还有就是我的滤波器能否还可以再改善

Answer 1

以下内容部分参考ChatGPT模型：

---

首先，根据提供的代码和数据，我发现卡尔曼滤波器的初始化有些问题，应该将dim_x设置为3，而不是2。因为我们要过滤的是三轴加速度，所以状态向量应该有三个分量。同时，根据系统模型，状态转移矩阵F应该改为：

kf.F = np.array([[1., dt, 0.5*dt*dt], [0., 1., dt], [0., 0., 1.]])

其中，dt是两个采样点之间的时间间隔，可以从数据中计算得到。这个矩阵表示当前时刻的状态向量与上一个时刻的状态向量之间的转移关系。

其次，在更新状态向量之前，应该先将加速度数据进行单位转换，从m/s^2转换为g。这可以通过乘以一个常数来完成。

最后，我发现在绘制图表时，y轴的标签应该是Acceleration (m/s^2)，而不是Acceleration (g)。另外，z轴的结果可能会有一些误差，因为z轴的加速度通常会受到重力的影响，需要进行一些特殊处理。

下面是修改后的代码示例：

import numpy as np
from filterpy.kalman import KalmanFilter
from scipy.signal import savgol_filter
import matplotlib.pyplot as plt

# 加载数据
data = np.loadtxt('data.txt', delimiter='\t')

# 初始化Kalman滤波器
kf = KalmanFilter(dim_x=3, dim_z=3)
kf.F = np.array([[1., dt, 0.5*dt*dt], [0., 1., dt], [0., 0., 1.]])
kf.H = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
kf.Q = np.diag([0.1, 0.1, 0.1])
kf.R = np.diag([10., 10., 10.])
kf.x = np.array([data[0, 0], 0., 0.])
kf.P = np.diag([10., 10., 10.])

# 对数据进行滤波
filtered_data = []
for i in range(data.shape[0]):
    kf.predict()
    kf.update(data[i, :3] * 9.81)  # 将加速度转换为m/s^2
    filtered_state = np.concatenate([kf.x, kf.P.diagonal()])
    filtered_data.append(np.concatenate([filtered_state, data[i, 3:]]))

filtered_data = np.array(filtered_data)

# 保存滤波后的数据到文件
np.savetxt('filtered_data.txt', filtered_data, delimiter='\t')

# 绘制图表
fig, axs = plt.subplots(3, 1, figsize=(10, 10))
for i in range(3):
    axs[i].plot(data[:, i], label='Raw Data')
    axs[i].plot(np.array(filtered_data)[:, i], label='Filtered Data')
    axs[i].legend()
    axs[i].set_xlabel('Time')
    axs[i].set_ylabel('Acceleration (m/s^2)')
    axs[i].set_title(f'Acceleration in {["X", "Y", "Z"][i]}')

plt.show()

---

如果我的建议对您有帮助、请点击采纳、祝您生活愉快