预测过程:
更新过程:
Python代码如下:
from matplotlib import pyplot as plt
import numpy as np
# 一维卡尔曼滤波,如果是高维,则需要进行矩阵操作
def kalman():
# 卡尔曼滤波的最终结果
x_kalman = []
''' 1.选择状态量,观测量 '''
# 状态量、观测量都为温度 y
''' 2.初始化参数 '''
# 所有的参数有 状态量x、观测量z、状态转移矩阵F、控制矩阵B、输入ut、观测矩阵H、先验估计协方差P、过程噪声Q、观测噪声R、kalman增益K
F = 1
H = 1
P = 1
# 设置x0=0
x = 0
# 用户指定
Q = 0 # Q为过程噪声(外部干扰),Q越大,表示在测量的过程中干扰越大,滤波效果就越不好
R = 0.01 # R为观测噪声,R取值越小,表示越信任传感器采集的值;R取值越大,表示越信任预测的值
''' 3.构建方程 '''
''' 4.迭代 '''
for i in range(len(x_Noise)):
x = F*x
# z为观测量
z = H*x_Noise[i]
P = F*P*F+Q
K = (P*H)/(H*P*H+R)
x = x+K*(z-H*x)
P = (1-K)*P
x_kalman.append(x)
print("观测值={}, 修正值={}".format(z, x))
''' 5.调节超参数(Q, R) '''
return x_kalman
# 真实数据
x_real = np.array([25 for i in range(100)])
# 添加均值μ=0,标准差std=3,数量为100且符合正态分布的随机噪声
Noise = np.random.normal(0.0, 3, 100)
# 观测数据
x_Noise = x_real + Noise
# 将数据进行卡尔曼滤波
x_kalman = kalman()
plt.figure(figsize=(8,5))
plt.plot(range(len(x_real)), x_real, label='real_data')
plt.plot(range(len(x_Noise)), x_Noise, label='Collect_data')
plt.plot(range(len(x_kalman)), x_kalman, label='kalman_data')
plt.legend()
plt.show()
一维卡尔曼滤波,如果要使用多维,就要进行矩阵的运算。
上述代码中,首先假设在某一时间段内(0-t)房间的真实温度为25度,实际使用传感器测量的温度是对25添加均值为0,方差为3符合正态分布的噪声,使用卡尔曼滤波对模拟实际采集的数据进行卡尔曼滤波。
结果如下图:



