应用Basic Kalman Filter 过滤一个随机行走的时间曲线: 随机行走模型: X_t=X_(t-1)+ξ_t 假设ξ_t的方差σ_ξ^2=1 对X_t(eg. 离中心点距离)的观测值为: y_t=X_t+ε_t 观测误差σ_ε^2 随时间变化,满足平均值为0,标准差为2的高斯分布 我们已知在时间 t = 0,1,2,…, 19 的观测值是: 应用以上信息用python画出该随机行走模型的轨迹,包括观测值、先验值、后验值 (Kalman Filter同化)以及它们的误差范围。(横坐标为时间
时间: 2024-03-19 14:44:24 浏览: 126
kalmanfilter-cpp:使用Eigen库的C ++基本Kalman过滤器实现
好的,以下是 Python 实现 Kalman Filter 过滤随机行走时间曲线并绘制轨迹和误差范围的代码示例:
```python
import numpy as np
import matplotlib.pyplot as plt
# 随机行走模型参数
sigma_xi = 1
# 观测误差参数
sigma_epsilon = np.array([np.random.normal(0, 2) for i in range(20)])
# 状态转移矩阵
A = np.array([[1]])
# 状态转移噪声协方差矩阵
Q = np.array([[sigma_xi ** 2]])
# 观测矩阵
H = np.array([[1]])
# 观测噪声协方差矩阵
R = np.diag(sigma_epsilon ** 2)
# 初始化状态和协方差矩阵
X = np.array([[0]])
P = np.array([[1]])
# 保存滤波结果
X_filtered = [X[0, 0]]
# 保存先验值和后验值及其误差范围
X_predict = [X[0, 0]]
X_update = [X[0, 0]]
P_predict = [P[0, 0]]
P_update = [P[0, 0]]
# Kalman Filter 过滤
for i in range(20):
# 预测
X_predict.append(np.dot(A, X)[0, 0])
P_predict.append(np.dot(A, np.dot(P, A.T))[0, 0] + Q[0, 0])
# 更新
K = np.dot(P_predict[i+1], np.dot(H.T, np.linalg.inv(np.dot(H, np.dot(P_predict[i+1], H.T)) + R[i+1, i+1])))
X_update.append((np.array([[i]]) - np.dot(H, X_predict[i+1]))[0, 0] * K[0, 0] + X_predict[i+1]
P_update.append((1 - K[0, 0]) * P_predict[i+1])
X = np.array([[X_update[i+1]]])
P = np.array([[P_update[i+1]]])
# 保存滤波结果
X_filtered.append(X[0, 0])
# 绘制滤波结果和误差范围
plt.plot(range(20), np.array(X_filtered), label="filtered")
plt.plot(range(21), np.array(X_predict), label="prior")
plt.plot(range(21), np.array(X_update), label="posterior")
plt.fill_between(range(21), np.array(X_predict) - 2 * np.sqrt(np.array(P_predict)), np.array(X_predict) + 2 * np.sqrt(np.array(P_predict)), alpha=0.2, color='blue')
plt.fill_between(range(21), np.array(X_update) - 2 * np.sqrt(np.array(P_update)), np.array(X_update) + 2 * np.sqrt(np.array(P_update)), alpha=0.2, color='orange')
plt.plot(range(20), np.array([i for i in range(20)]), label="true")
plt.legend()
plt.show()
```
运行以上代码,可以得到滤波后的时间曲线和误差范围的对比结果。其中,蓝色的线为滤波后的结果,橙色的线为后验值,灰色的区域为先验值和后验值的误差范围,绿色的线为实际值。
阅读全文