Kalman和一阶互补滤波滤波代码

卡尔曼滤波（Kalman Filter）是一种广泛用于估计动态系统状态的线性滤波算法，它结合了系统的模型预测和观测数据更新，适用于处理噪声信号和不确定性。一阶互补滤波，也称为粒子滤波（Complementary Filter），常用于惯性测量单元（IMU）的数据融合，将加速度计和陀螺仪的数据结合起来提高方向估计的精度。

以下是简化的Python代码示例，展示了如何实现一阶互补滤波：

import numpy as np

class ComplementaryFilter:
    def __init__(self, alpha):
        self.alpha = alpha  # 加权因子，一般0 < α < 1
        self.acceleration = 0
        self.angular_rate = 0

    def update_acceleration(self, accel_data):
        self.acceleration = self.alpha * accel_data + (1 - self.alpha) * self.acceleration

    def update_angular_rate(self, gyro_data):
        self.angular_rate = self.alpha * gyro_data + (1 - self.alpha) * self.angular_rate

    def get_filtered_orientation(self, initial_orientation):
        orientation = initial_orientation
        dt = 1.0  # 时间步长假设为1秒
        orientation += self.acceleration * dt**2 / 2  # 使用加速度积分
        orientation += self.angular_rate * dt  # 使用角速度直接累加
        return orientation

# 使用示例
filter = ComplementaryFilter(alpha=0.98)
accel_data = [0.5, 0.1, -0.3]  # 某时刻的加速度数据
gyro_data = [0.2, -0.1, 0.4]   # 某时刻的角速度数据
orientation = filter.get_filtered_orientation(initial_orientation=0)  # 初始姿态为0