生成关于卡尔曼滤波的代码
时间: 2023-12-02 20:41:30 浏览: 72
卡尔曼滤波的代码
根据您提供的引用内容,您的问题是关于贝叶斯滤波和多传感器数据融合的,而不是卡尔曼滤波。以下是一个生成随机信号并使用卡尔曼滤波进行滤波的Python代码示例:
```python
import numpy as np
import matplotlib.pyplot as plt
# 生成随机信号
np.random.seed(0)
n_samples = 200
x = np.linspace(0, 10, n_samples)
y = np.sin(x) + np.random.randn(n_samples) * 0.1
# 定义卡尔曼滤波器
class KalmanFilter:
def __init__(self, F, H, Q, R, x0, P0):
self.F = F
self.H = H
self.Q = Q
self.R = R
self.x = x0
self.P = P0
def predict(self):
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
def update(self, z):
y = z - self.H @ self.x
S = self.H @ self.P @ self.H.T + self.R
K = self.P @ self.H.T @ np.linalg.inv(S)
self.x = self.x + K @ y
self.P = (np.eye(self.F.shape[0]) - K @ self.H) @ self.P
# 初始化卡尔曼滤波器
F = np.array([[1, 1], [0, 1]])
H = np.array([[1, 0]])
Q = np.diag([0.1, 0.1])
R = np.array([[0.1]])
x0 = np.array([0, 0])
P0 = np.diag([1, 1])
kf = KalmanFilter(F, H, Q, R, x0, P0)
# 使用卡尔曼滤波器进行滤波
filtered_y = []
for i in range(n_samples):
kf.predict()
kf.update(y[i])
filtered_y.append(kf.x[0])
# 绘制结果
plt.plot(x, y, label='Original signal')
plt.plot(x, filtered_y, label='Filtered signal')
plt.legend()
plt.show()
```
阅读全文