- 根据k-1时刻的最优估计值来预测k时刻的预测值
- 再根据k时刻的测量值和k时刻的预测值，得到此时刻的最佳估计值
- 循环迭代
- 需要说明的是，不管是根据上一时刻的预测值还是本时刻的测量值，都是有误差存在的
假设我们要研究房间里的温度，以分钟为单位。因为房间里的温度变化不大，因此根据k-1时刻的最优估计值来预测k时刻的预测值时，我们由经验认为：
=
但是这个预测是有误差的，我们把误差定义为高斯白噪声。另外，房间里放置一个温度计，但是温度计的测量值也是有误差的，误差也认为是高斯噪声。
现在我们的任务是：根据k时刻经验预测值和温度计的测量值，以及其各自的误差分布，得到k时刻的最优估计值。
第一步：
假设，k-1时刻的最优估计值为23°，那么=23°。k时刻温度计的测量值为25°。
假设经验预测值的高斯白噪声误差是5°（这个误差一直想不明白是怎么来的）
温度计测量的误差为4°。（4°为测量仪器的不确定度误差，是仪器本身给定的）
第二步：
那么预测的23°和测量的25°我们更应该相信哪个呢，这要根据他们误差的大小5°和4°来决定。
计算卡尔曼增益：Kg = =0.78
第三步：
k时刻的最优估计值=23+0.78（25-23）= 24.56°
- 从程序上可以看出来，预测和测量的误差的大小是根据Kg和预测和测量值来更新的，因此，只需要初始时刻有一个5°和4°即可，程序里把初始时刻的预测误差大小和测量误差都定义为0.1
第四步：
更新预测误差和测量误差
预测误差更新：°
测量误差更新：°
程序
from matplotlib import pyplot
import math
import random
lastTimePredVal = 0 # 上次估计值
lastTimePredCovVal = 0.1 # 上次估计协方差
lastTimeRealCovVal = 0.1 # 上次实际协方差
kg = 0.0 #卡尔曼增益
# val: 本次测量值
def kalman(val):
#python中如果若想在函数内部对函数外的变量进行操作，就需要在函数内部声明其为global。
global lastTimePredVal # 上次估计值
global lastTimePredCovVal # 上次估计协方差
global lastTimeRealCovVal # 上次实际协方差
global kg
currRealVal = val # 本次实际值
currPredCovVal = lastTimePredCovVal # 本次估计协方差值
currRealCovVal = lastTimeRealCovVal # 本次实际协方差值
# 计算本次估计值，并更新保留上次预测值的变量
currPredVal = lastTimePredVal + kg * (currRealVal - lastTimePredVal)
lastTimePredVal = currPredVal
#计算卡尔曼增益
kg = math.sqrt(math.pow(lastTimePredCovVal, 2) / (math.pow(lastTimePredCovVal, 2) + math.pow(lastTimeRealCovVal, 2)))
# 计算下次估计和实际协方差
lastTimePredCovVal = math.sqrt(1.0 - kg) * currPredCovVal
lastTimeRealCovVal = math.sqrt(1.0 - kg) * currRealCovVal
# 返回本次的估计值,也就是滤波输出值
return currPredVal
if __name__ == "__main__":
realTemp = [] # 真实温度
predTemp = [] # 预测温度
# 生成50个真实温度，20度到23度之间
for i in range(50):
realTemp.append(random.uniform(20, 23))
# 卡尔曼滤波
for t in realTemp:
predVal = kalman(t)
predTemp.append(predVal)
# 绘制真实温度和预测温度折线图
pyplot.figure()
pyplot.plot(predTemp, label='predict_temp')
pyplot.plot(realTemp, label='real_temp')
pyplot.tick_params(axis='x', which='major', labelsize=int(len(predTemp)/10))
pyplot.xlabel('Count')
pyplot.ylabel('Temperature')
pyplot.show()
参考文章来源：
https://blog.csdn.net/phker/article/details/48468591
https://blog.csdn.net/qq153471503/article/details/102702368?utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7Edefault-13.no_search_link&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7Edefault-13.no_search_link
