卡尔曼滤波总结以及python实现卡尔曼滤波

1. 前言

卡尔曼滤波是一种用于估计未知状态的滤波算法。它可以从带有噪声的传感器数据中提取出实际的状态变量。

卡尔曼滤波的基本思想是使用已知的状态和测量值来预测未来的状态，同时考虑测量噪声和系统噪声对估计值的影响。

卡尔曼滤波算法由两个步骤组成：预测和更新；

在预测步骤中，卡尔曼滤波器基于先前的状态和系统的动态模型来预测未来的状态。这个预测可以通过使用状态转移矩阵（描述系统的动态行为）和旧状态来完成；

在更新步骤中，卡尔曼滤波器使用测量结果来校正预测。这个校正可以通过计算测量噪声和状态估计值的协方差矩阵来完成。然后，卡尔曼滤波器将当前的状态更新为调整后的状态。

卡尔曼滤波器可以有效地处理多个传感器数据和噪声源的复杂情况，它可以自适应地根据当前情况进行调整。

2. python简单实现卡尔曼滤波

# 这个例子使用了一个一维的状态变量（位置和速度），
# 并假设控制输入为零。在实际应用中，
# 控制输入可能是一个外部变量（比如加速度），
# 可以通过调整控制输入矩阵来处理。
# @ 1.用来表示修饰符，传参作用； 2.表示矩阵相乘。  这里的@是表示矩阵相乘

import numpy as np

# 定义一些常量
dt = 1        # 时间步长
A = np.array([[1, dt], [0, 1]])       # 状态转移矩阵
B = np.array([[0], [0]])              # 控制输入矩阵 (没有用到)
H = np.array([[1, 0]])                # 测量矩阵
q = 1e-5     # 系统噪声方差
r = 0.1      # 测量噪声方差

# 定义初始状态
x = np.array([[0], [0]])   # 状态变量 (位置和速度)
P = np.diag([1000, 1000])  # 状态协方差矩阵

# 定义测量结果
measurements = [1, 2, 3, 4, 5]

# 卡尔曼滤波
for measurement in measurements:
    # 预测步骤
    x_pred = A @ x             # 预测状态
    P_pred = A @ P @ A.T + q   # 预测状态协方差矩阵

    # 更新步骤
    K = P_pred @ H.T / (H @ P_pred @ H.T + r)   # 卡尔曼增益
    x = x_pred + K * (measurement - H @ x_pred)  # 校正状态
    P = (np.eye(2) - K @ H) @ P_pred            # 校正状态协方差矩阵

    # 输出结果
    print("Measurement:", measurement)
    print("Predicted state:", x_pred)
    print("Updated state:", x)
    print("---------------------------")

3. python实现带可视化卡尔曼滤波

# 这个例子使用了一个一维的状态变量（位置和速度），并假设控制输入为零。
# 它首先生成了一个正弦波形的实际状态，并通过添加随机噪声来模拟测量结果。
# 然后，它运行了卡尔曼滤波器来估计实际状态，并将其与测量结果和真实状态进行比较

import numpy as np
import matplotlib.pyplot as plt

# 定义一些常量
dt = 0.1       # 时间步长
A = np.array([[1, dt], [0, 1]])       # 状态转移矩阵
B = np.array([[0], [0]])              # 控制输入矩阵 (没有用到)
H = np.array([[1, 0]])                # 测量矩阵
q = 1e-5     # 系统噪声方差
r = 0.1      # 测量噪声方差

# 定义初始状态
x = np.array([[0], [0]])   # 状态变量 (位置和速度)
P = np.diag([1000, 1000])  # 状态协方差矩阵

# 定义测量结果
measurements = np.arange(0, 10, dt)
n = len(measurements)
real_state = 2 * np.sin(0.1 * measurements)  # 实际状态

# 生成随机噪声
np.random.seed(0)
q_noise = np.random.normal(0, q, n)   # 系统噪声
r_noise = np.random.normal(0, r, n)   # 测量噪声
measurements += r_noise

# 卡尔曼滤波
states = np.zeros((n, 2))
for i in range(n):
    # 预测步骤
    x_pred = A @ x             # 预测状态
    P_pred = A @ P @ A.T + q   # 预测状态协方差矩阵

    # 更新步骤
    K = P_pred @ H.T / (H @ P_pred @ H.T + r)   # 卡尔曼增益
    x = x_pred + K * (measurements[i] - H @ x_pred)  # 校正状态
    P = (np.eye(2) - K @ H) @ P_pred             # 校正状态协方差矩阵

    # 保存状态
    states[i, :] = x[:, 0]

# 绘制结果
plt.plot(measurements, 'k+', label='Measurements')
plt.plot(states[:, 0], 'b-', label='Kalman Filter')
plt.plot(real_state, 'g-', label='Real state')
plt.legend(loc='upper left')
plt.title('Kalman Filter Results')
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.show()

参考：
[1] 代码参考来自chatgpt4.