卡尔曼滤波做数据融合实例python代码实现

卡尔曼滤波是一种递归最小二乘算法，常用于估计动态系统中的状态变量，特别是在存在噪声的情况下。在Python中，我们可以使用`scipy.signal`库中的`kalman_filter`函数来实现数据融合。这里是一个简单的例子：

import numpy as np
from scipy.signal import kalman_filter

# 假设我们有一个线性模型
def motion_model(state, dt, u=0):
    return state + dt * u

# 状态转移矩阵A和过程噪声Q
A = np.array([[1, dt], [0, 1]])
Q = np.eye(2) * 0.1**2

# 测量模型H和测量噪声R
H = np.array([[1, 0]])  # 假设我们的测量只有位置信息
R = np.array([[1e-3]])   # 测量误差假设很小

# 初始状态和状态协方差P
x0 = np.array([0., 0.])
P0 = np.eye(2) * 10**2

# 测试数据
measurements = np.random.normal(0, 1, (100, 1))
u = np.zeros_like(measurements)

# 创建Kalman Filter对象并应用数据融合
kf = kalman_filter(A, H, Q, R, P0, x0)
filtered_states, _ = kf.filter(measurements, args=(u,))

print("Filtered states:", filtered_states)

在这个例子中，我们创建了一个简单的卡尔曼滤波器来融合传感器测量数据。`kalman_filter`函数返回两个数组，一个是经过滤波的状态序列，另一个是对应的时间戳。