卡尔曼滤波通俗易懂原理及其python实现

kalman滤波的核心思想

  • 根据k-1时刻的最优估计值x_{k-1}来预测k时刻的预测值\hat{x}_{k|k-1}
  • 再根据k时刻的测量值Z_{k}和k时刻的预测值\hat{x}_{k|k-1},得到此时刻的最佳估计值x_{k}
  • 循环迭代
  • 需要说明的是,不管是根据上一时刻的预测值还是本时刻的测量值,都是有误差存在的

举例

        假设我们要研究房间里的温度,以分钟为单位。因为房间里的温度变化不大,因此根据k-1时刻的最优估计值x_{k-1}来预测k时刻的预测值\hat{x}_{k|k-1}时,我们由经验认为:

        x_{k-1} = \hat{x}_{k|k-1} 

        但是这个预测是有误差的,我们把误差定义为高斯白噪声。另外,房间里放置一个温度计,但是温度计的测量值也是有误差的,误差也认为是高斯噪声。

        现在我们的任务是:根据k时刻经验预测值和温度计的测量值,以及其各自的误差分布,得到k时刻的最优估计值。


第一步:

假设,k-1时刻的最优估计值x_{k-1}为23°,那么\hat{x}_{k|k-1}=23°。k时刻温度计的测量值Z_{k}为25°。

假设经验预测值\hat{x}_{k|k-1}的高斯白噪声误差是5°(这个误差一直想不明白是怎么来的)

温度计测量的误差为4°。(4°为测量仪器的不确定度误差,是仪器本身给定的)


 第二步:

那么预测的23°和测量的25°我们更应该相信哪个呢,这要根据他们误差的大小5°和4°来决定。

计算卡尔曼增益:Kg = \frac{5^2}{5^2+4^2}=0.78


第三步:

k时刻的最优估计值x_{k}=23+0.78(25-23)= 24.56°

  • 从程序上可以看出来,预测和测量的误差的大小是根据Kg和预测和测量值来更新的,因此,只需要初始时刻有一个5°和4°即可,程序里把初始时刻的预测误差大小和测量误差都定义为0.1

第四步:

 更新预测误差和测量误差

预测误差更新:\sqrt{1-Kg}\times 5°

 测量误差更新:\sqrt{1-Kg}\times 4°

 程序

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()

 参考文章来源:

卡尔曼滤波,最最容易理解的讲解.找遍网上就这篇看懂了._走错路的程序员-CSDN博客

 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

你可能感兴趣的:(卫星导航,python,卡尔曼滤波算法)