使用卡尔曼滤波算法-过滤加速度X数据-Python

卡尔曼滤波可预测波形过滤波形。

串口接收数据格式

从串口数据收到如下: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的设定需要你对系统动力学有深入的理解。你也可能需要进行多次试验并观察结果，以便微调这些参数以获得最佳表现。