卡尔曼滤波通俗易懂理解和代码实现

卡尔曼滤波算法应用—滤波单片机采集的温度


基本讲解在这里:https://blog.csdn.net/phker/article/details/48468591,网上搜了这么多文章,就这篇博客讲的很直白,通俗易懂。


公式

在这里插入图片描述
在这里插入图片描述

看公式1,ECR是估计变化比,也就是一个系统参数,需要我们根据实际情况调的。因为我们只有一个温度传感器,假如是测室温,那么我们认为短时间内室温是恒定的,所以这个变化比就等一。

将上面的公式转为汉字可以理解为:

公式1:

	本次估计值 = 估计变化比 * 上次估计值 + 卡尔曼增益 * (测量值 - 上次估计值)

公式2:
	
	卡尔曼增益 = 根号下(当前估计协方差的平方 / (当前估计协方差的平方 + 当前测量测方差的平方))

公式3:
	
	下次估计协方差 = [根号下(1 - 卡尔曼增益)] * 当前估计协方差

公式4:

	下次测量协方差 = [根号下(1 - 卡尔曼增益)] * 当前测量协方差




算法步骤:

1、初始化:

	上次估计值 = 0
	当前估计协方差 = 0.1 (任意不为0的值)
	当前测量协方差 = 0.2 (任意不为0初值)

2、计算

	1、获得测量值(也就是温度传感器测到的温度)
	2、计算卡尔曼增益(公式二)
	3、计算当前估计值,并将当前估计值赋值给上次估计变量,用于下次使用
	4、利用公式三和四,计算下次估计协方差和下次测量协方差
	5、更新协方差: 下次估计协方差 = 当前估计协方差,下次测量协方差 = 当前估计协方差
	6、返回本次估计值,也就是返回本次滤波之后的温度

Python代码

from matplotlib import pyplot
import math
import random

lastTimePredVal = 0  # 上次估计值
lastTimePredCovVal = 0.1  # 上次估计协方差
lastTimeRealCovVal = 0.1  # 上次实际协方差
kg = 0.0

# val: 本次测量值
def kalman(val):

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

红线代表实际测量的温度,蓝线代表滤波的温度。
在这里插入图片描述

C代码

typedef struct
{
    float lastTimePredVal;            // 上次估计值
    float lastTimePredCovVal;         // 上次估计协方差
    float lastTimeRealCovVal;         // 上次实际协方差
    float kg;
}kalman_typedef;

void kalman_init(kalman_typedef* kalman)
{
	kalman->lastTimePredVal = 0;
	kalman->lastTimePredCovVal = 0.1;
	kalman->lastTimeRealCovVal = 0.1;
	kalman->kg = 0;
}

// val: 本次测量值
float kalman_calc(kalman_typedef* kalman, float val)
{
    float currPredVal = 0;                             // 本次估计值
    float currRealVal = val;                           // 本次实际值
    float currPredCovVal = kalman->lastTimePredCovVal;         // 本次估计协方差值
    float currRealCovVal = kalman->lastTimeRealCovVal;         // 本次实际协方差值

    // 计算本次估计值,并更新保留上次预测值的变量
    currPredVal = kalman->lastTimePredVal + kalman->kg * (currRealVal - kalman->lastTimePredVal);
    kalman->lastTimePredVal = currPredVal;

    //计算卡尔曼增益
    kalman->kg = sqrt(pow(kalman->lastTimePredCovVal, 2) / (pow(kalman->lastTimePredCovVal, 2) + pow(kalman->lastTimeRealCovVal, 2)));

    // 计算下次估计和实际协方差
    kalman->lastTimePredCovVal = sqrt(1.0 - kalman->kg) * currPredCovVal;
    kalman->lastTimeRealCovVal = sqrt(1.0 - kalman->kg) * currRealCovVal;

    // 返回本次的估计值,也就是滤波输出值
    return currPredVal;
}


ends…

  • 5
    点赞
  • 80
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 2
    评论
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

觉皇嵌入式

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值