卡尔曼滤波代码
时间: 2023-07-05 20:22:01 浏览: 42
卡尔曼滤波是一种用于状态估计和信号滤波的常用方法,下面是一个简单的基于Python实现的卡尔曼滤波代码示例:
```python
import numpy as np
def kalman_filter(x, P, measurement, R, Q, F, H, B, u):
# 预测
x = np.dot(F, x) + np.dot(B, u)
P = np.dot(np.dot(F, P), F.T) + Q
# 更新
S = np.dot(np.dot(H, P), H.T) + R
K = np.dot(np.dot(P, H.T), np.linalg.inv(S))
y = measurement - np.dot(H, x)
x = x + np.dot(K, y)
P = np.dot(np.eye(len(x)) - np.dot(K, H), P)
return x, P
# 初始化
dt = 0.1
F = np.array([[1, dt], [0, 1]]) # 状态转移矩阵
H = np.array([[1, 0]]) # 观测矩阵
P = np.diag((0.01, 0.01)) # 状态协方差矩阵
Q = np.eye(2) * 0.001 # 过程噪声协方差矩阵
R = np.eye(1) * 0.1 # 测量噪声协方差矩阵
B = np.array([[0.5*dt**2], [dt]]) # 外部输入矩阵
u = np.array([[0], [0]]) # 外部输入向量
# 测试数据
measurements = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]
# 迭代卡尔曼滤波
x = np.array([[measurements[0]], [0]])
result = []
for measurement in measurements:
x, P = kalman_filter(x, P, measurement, R, Q, F, H, B, u)
result.append(x[0][0])
# 输出结果
print(result)
```
以上代码实现了一个简单的一维卡尔曼滤波器,用于对一组模拟数据进行滤波。在实际应用中,需要根据具体问题进行参数调整和代码实现。