卡尔曼滤波器最佳效果（多传感器融合卡尔曼滤波）
卡尔曼滤波器最佳效果（多传感器融合卡尔曼滤波）
今天，铁血使命掌握卡尔曼滤波核心应用技术。卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。 该技术在目标追踪、航天领域等具有重大意义。
研究背景：多个一样的传感器融合问题，估计最优值。
两个传感器的测量值分别 服从N（1，0.5），N（1，1）的正态分布
import numpy as np
from matplotlib import pyplot as plt
size = 500
t = [i for i in range(size)]
plt.figure(figsize=(20 10) dpi=100)
# 观测值 产生 两个传感器的测量值 均值为1标准差分别为1 ，0.5
sgma1 = 1.0
sgma2 = 0.5
s1 = np.random.normal(1 sgma1 size=size)
s2 = np.random.normal(1 sgma2 size=size)
k1 = sgma2/(sgma1 sgma2)
k2 = sgma1/(sgma1 sgma2)
res = []
X = np.array([0 0 0]).T # 状态
P = np.array([[1 0 0] [0 1 0] [0 0 1]]) # 状态协方差矩阵
A = np.array([[0 k1 k2] [0 1 0] [0 0 1]]) # 状态转移矩阵 每秒采一次样
Q = np.array([[0.0001 0 0] [0 0.0001 0] [0 0 0.0001]])
C = np.array([[0 1 0] [0 0 1]]) # 观测矩阵
R = np.array([[sgma1**2 0] [0 sgma2**2]]) # 观测噪声方差
y = np.array([[s1[i] s2[i]] for i in range(size)])
for i in range(size):
X_ = A @ X
P_ = A @ P @ A.T Q
K = P_@C.T @ np.linalg.inv(C@P_@C.T R)
z = y[i].T - C @ X_
X = X_ K@z
P = (np.eye(3) - K.reshape(3 2)@C.reshape(2 3)) @ P_
plt.ylim(0 1.5)
res.append(X[0])
point1 = plt.scatter(t s1 c='r' linewidths=2)
point2 = plt.scatter(t s2 c='b' linewidths=2)
line = plt.plot(t res c='g' linewidth=3)
plt.legend(handles=[point1 point2 line] labels=["sensor s1" "sensor2 s1" "optimal value" ] loc="lower right" fontsize=15)
#plt.legend(handles=[line ] labels=[ "optimal value" ] loc="lower right" fontsize=15)
plt.xlabel('t/s')
plt.savefig("./img")
plt.show()
结果：收敛到1附近
