卡尔曼滤波可预测波形过滤波形。
卡尔曼滤波算法验证
串口接收数据格式
从串口数据收到如下:xyz加速度,xyz角速度,温度,速度变化量,速度变化量的矢量
[17:47:35.605]收←◆27 -289 915 0 0 2 24.7 0 1 1 1
[17:47:35.706]收←◆-6 -294 953 0 0 0 24.7 0 0 2 2
[17:47:35.825]收←◆15 -274 955 0 0 1 24.7 0 1 0 1
[17:47:35.945]收←◆12 -245 957 0 0 1 24.7 0 1 1 1
[17:47:36.065]收←◆18 -276 938 0 0 -1 24.7 0 0 0 0
[17:47:36.165]收←◆14 -268 923 0 0 -2 24.7 0 0 0 0
[17:47:36.285]收←◆31 -255 957 0 0 -2 24.7 0 2 0 2
[17:47:36.406]收←◆11 -271 964 0 0 1 24.7 0 0 1 1
[17:47:36.525]收←◆14 -257 937 0 0 -2 24.7 0 1 0 1
[17:47:36.627]收←◆15 -271 932 0 0 -1 24.7 0 1 1 1
[17:47:36.746]收←◆1 -274 945 0 0 0 24.7 1 1 1 1
[17:47:36.865]收←◆24 -257 923 0 0 -1 24.7 0 1 1 1
[17:47:36.985]收←◆2 -277 926 0 0 0 24.7 0 1 1 1
[17:47:37.085]收←◆18 -276 942 0 0 0 24.7 0 0 0 0
[17:47:37.205]收←◆-7 -265 928 0 0 0 24.7 0 0 0 0
[17:47:37.325]收←◆14 -261 923 0 0 2 24.7 1 0 0 1
[17:47:37.446]收←◆11 -291 938 0 0 0 24.7 0 1 0 1
下面是卡尔曼滤波程序:
- 从串口获取传感器数据
- 读取X加速度,并进行滤波
- 画图显示更新
- 串口收到数据如下:
import serial
import matplotlib.pyplot as plt
import numpy as np
class KalmanFilter:
def __init__(self, R, Q, A, B, C, x, P):
self.R = R # prediction noise
self.Q = Q # sensor noise
self.A = A # state transition model
self.B = B # control input model
self.C = C # observation model
self.current_state_estimate = x
self.current_prob_estimate = P
def current_state(self):
return self.current_state_estimate
def step(self, control_input, measurement):
# Predict the state
predicted_state_estimate = self.A * self.current_state_estimate + self.B * control_input
predicted_prob_estimate = (self.A * self.current_prob_estimate) * self.A + self.R
# Compute the observed state
observation_transpose = (1. / (self.C * predicted_prob_estimate * self.C + self.Q))
kalman_gain = predicted_prob_estimate * self.C * observation_transpose
innovation_estimate = measurement - self.C * predicted_state_estimate
# Correct / update the prediction
self.current_state_estimate = predicted_state_estimate + kalman_gain * innovation_estimate
self.current_prob_estimate = (1 - kalman_gain * self.C) * predicted_prob_estimate
num_lines = 1
num_data = num_lines
# 创建串口对象
ser = serial.Serial("COM4", 115200)
plt.ion() # 开启交互模式
# 创建6个子图,分别对应加速度和角速度的三个方向
fig, axs = plt.subplots(6)
fig.suptitle('Accelerometer and Gyroscope Readings')
accel_x, accel_y, accel_z = [], [], []
gyro_x, gyro_y, gyro_z = [], [], []
def read_serial_data(num_lines, num_data):
global accel_x, accel_y, accel_z
global gyro_x, gyro_y, gyro_z
for _ in range(num_lines):
# 从串口读取一行
line = ser.readline()
# 解码并分割数据
data_strings = line.decode('utf-8').strip().split()
# 转换到特定的数据类型
data = (
tuple(map(int, data_strings[0:3])), # x,y,z加速度
tuple(map(int, data_strings[3:6])), # x,y,z角速度
)
# 更新数据列表
accel_x.append(data[0][0])
accel_y.append(data[0][1])
accel_z.append(data[0][2])
gyro_x.append(data[1][0])
gyro_y.append(data[1][1])
gyro_z.append(data[1][2])
# 更新所有的图表
axs[0].clear()
axs[0].set_title('Accelerometer X')
axs[0].plot(accel_x)
axs[1].clear()
axs[1].set_title('Accelerometer Y')
axs[1].plot(accel_y)
axs[2].clear()
axs[2].set_title('Accelerometer Z')
axs[2].plot(accel_z)
axs[3].clear()
axs[3].set_title('Gyroscope X')
axs[3].plot(gyro_x)
axs[4].clear()
axs[4].set_title('Gyroscope Y')
axs[4].plot(gyro_y)
axs[5].clear()
axs[5].set_title('Gyroscope Z')
axs[5].plot(gyro_z)
plt.draw() # 显示图形
plt.pause(0.1)
# 创建单一的子图,用于显示加速度X的原始数据和滤波后的数据
fig, ax = plt.subplots()
fig.suptitle('Accelerometer X - Raw and Filtered')
# 调整read_serial_data以只更新加速度X
def read_serial_data(num_lines, num_data):
global accel_x
for _ in range(num_lines):
# 从串口读取一行
line = ser.readline()
# 解码并分割数据
data_strings = line.decode('utf-8').strip().split()
# 更新数据列表
accel_x.append(int(data_strings[0])) # assuming accel_x is the first data on each line
read_serial_data(num_lines, num_data)
# 使用accel_x的第一个值来初始化KalmanFilter
kalman_filter = KalmanFilter(
R= np.array([0.01]), # prediction noise
Q= np.array([0.1]), # sensor noise
A= np.array([1]), # state transition model
B= np.array([0]), # control input model
C= np.array([1]), # observation model
x= np.array([accel_x[0]]), # initial state estimate
P= np.array([1]) # initial state covariance
)
accel_x_filtered = []
while True:
read_serial_data(num_lines, num_data)
kalman_filter.step(
control_input=np.array([0]),
measurement=np.array([accel_x[-1]])
)
accel_x_filtered.append(kalman_filter.current_state())
# 我们现在只清除和绘制一个子图,加速度X
ax.clear()
ax.set_title('Accelerometer X')
ax.plot(accel_x, label='Raw') # Raw data
ax.plot(accel_x_filtered, label='Filtered') # Filtered data
ax.legend() # Add a legend to distinguish raw data and filtered data
plt.draw() # 显示图形
plt.pause(0.1)
卡尔曼滤波效果对比:橙色是滤波后
可调整参数
上述代码中定义的KalmanFilter类具有几个重要的参数,改变这些参数将会影响滤波器的行为:
R(预测噪声):这个参数对应的是模型假定的系统内部的噪声,换句话说,即使没有任何输入,模型仍然会预测一些变化。如果增加R,滤波器将会对预测值不那么自信,系统将相信观察值更多。
Q(观察噪声):这代表系统假定的测量设备的噪声。如果增加Q,滤波器会更加不相信观察值,而更多地相信自己的预测。
A,B,C(状态转移模型,控制输入模型,观察模型):这些参数用于描述系统的动态行为。A描述了如何从一个状态预测下一个状态,B描述了控制输入如何影响状态,C描述了如何从状态生成观察值。改变这些值将改变模型预测的系统行为。
x(初始状态估计)以及P(初始状态协方差):这些参数用于设置滤波器的初始状态。改变这些值将影响滤波器最初的估计。
对于加速度传感器和陀螺仪的读数,这些参数需要结合你熟悉的系统动态和观察来设定。比如,如果你知道加速度传感器非常准确(低噪声),那么你可以设定较小的Q值。如果你的传感器在没有运动的情况下读数波动很大,那么你可能需要设定一个较大的R值。A,B,C的设定需要你对系统动力学有深入的理解。你也可能需要进行多次试验并观察结果,以便微调这些参数以获得最佳表现。