2.4碎片

import numpy as np
import matplotlib.pyplot as plt

# 定义卡尔曼滤波器参数
sampling_time = 1  # 采样时间间隔
initial_state = np.array([[0], [0]])  # 初始状态
initial_covariance = np.eye(2)  # 初始协方差矩阵
process_noise = 0.1  # 系统噪声方差
measurement_noise = 1  # 测量噪声方差
transition_matrix = np.array([[1, sampling_time], [0, 1]])  # 状态转移矩阵
observation_matrix = np.array([[1, 0]])  # 观测矩阵

# 初始化卡尔曼滤波器
state = initial_state
covariance = initial_covariance

# 生成测量数据
actual_data = [(time, 10 * time + 5 + np.random.uniform(-2, 2)) for time in range(101)]
measurements = np.array([measurement for _, measurement in actual_data])

# 预测和更新
predicted_measurements = []
residuals = []
for measurement in measurements:
    # 预测
    predicted_state = np.dot(transition_matrix, state)
    predicted_covariance = np.dot(np.dot(transition_matrix, covariance), transition_matrix.T) + process_noise

    # 更新
    innovation = measurement - np.dot(observation_matrix, predicted_state)
    innovation_covariance = np.dot(np.dot(observation_matrix, predicted_covariance), observation_matrix.T) + measurement_noise
    kalman_gain = np.dot(np.dot(predicted_covariance, observation_matrix.T), np.linalg.inv(innovation_covariance))
    state = predicted_state + np.dot(kalman_gain, innovation)
    covariance = np.dot(np.eye(2) - np.dot(kalman_gain, observation_matrix), predicted_covariance)

    # 保存预测的测量值和残差
    predicted_measurement = np.dot(observation_matrix, state)
    predicted_measurements.append(predicted_measurement[0,0])
    residuals.append(measurement - predicted_measurement[0,0])

# 绘制残差与时间的关系图像
times = range(len(residuals))
plt.plot(times, residuals)
plt.xlabel('Time(s)')
# 添加Y=0的水平线
plt.axhline(y=0, color='r', linestyle='--')
plt.grid(True, linestyle='--')
plt.ylabel('Residuals(m)')
plt.show()