卡尔曼滤波

核心思想：根据当前的仪器"测量值" 和上一刻的 “预测量” 和 “误差”，计算得到当前的最优量，再预测下一刻的量。里面比较突出的是观点是：把误差纳入计算，而且分为预测误差和测量误差两种，通称为噪声。还有一个非常大的特点是：误差独立存在，始终不受测量数据的影响。

优点：巧妙的融合了观测数据与估计数据，对误差进行闭环管理，将误差限定在一定范围。适用性范围很广，时效性和效果都很优秀。

缺点：需要调参，参数的大小对滤波的效果影响较大。

卡尔曼滤波有一定的去噪稳定特性的，虽然效果不是特别优秀。卡尔曼滤波的普适性很强，尤其在控制与多传感器融合方向，只要参数调整的好，效果出奇优秀。

class KalmanFilter:
    def __init__(self, q=0.001, r=0.001):
        self.q = q  # 过程噪声协方差
        self.r = r  # 测量噪声协方差
        self.p = 5  # 估计误差协方差
        self.k_gain = 0  # 卡尔曼增益
        self.prev_data = 0  # 先前数据值

    def update(self, measurement):
        # 预测
        self.p += self.q
        # 计算卡尔曼增益
        self.k_gain = self.p / (self.p + self.r)
        # 更新估计值
        estimation = self.prev_data + self.k_gain * (measurement - self.prev_data)
        # 更新估计误差协方差
        self.p = (1 - self.k_gain) * self.p
        # 更新先前数据值
        self.prev_data = estimation

        return estimation

# 测试
kf = KalmanFilter(q=0.001, r=0.001)
input_data = 5
filtered_data = kf.update(input_data)
print("滤波后的值:", filtered_data)


import numpy as np
import matplotlib.pyplot as plt

# 生成模拟传感器数据（示例数据）
np.random.seed(123456)
sensor_data = np.random.randn(200)  # 正态分布随机数据


n = 11 # 在多少个数中取中间值,必须为奇数

filtered_sensor_data = np.zeros_like(sensor_data)
for i in range(1,len(sensor_data)):
    src_data = sensor_data[i]
    filtered_sensor_data[i] = kf.update(src_data)

# 绘制原始数据和滤波后数据
plt.figure(figsize=(10, 6))
plt.plot(sensor_data,c="green")
plt.plot(filtered_sensor_data,c="red")

plt.show()