2013-12-27 10:58:43 u011344545 阅读数 1333
  • 单片机控制第一个外设-LED灯-第1季第6部分

    本课程是《朱有鹏老师单片机完全学习系列课程》第1季第6个课程,主要讲解LED的工作原理和开发板原理图、实践编程等,通过学习目的是让大家学会给单片机编程控制LED灯,并且为进一步学习其他外设打好基础。

    4000 人正在学习 去看看 朱有鹏

博主github:https://github.com/MichaelBeechan
博主CSDN:https://blog.csdn.net/u011344545

//float gyro_m:陀螺仪测得的量(角速度)

//float incAngle:加计测得的角度值

#define dt                  0.0015//卡尔曼滤波采样频率

#define R_angle          0.69 //测量噪声的协方差(即是测量偏差)

#define Q_angle          0.0001//过程噪声的协方差

#define Q_gyro         0.0003 //过程噪声的协方差  过程噪声协方差为一个一行两列矩阵

float kalmanUpdate(const float gyro_m,constfloat incAngle) 

{        

       float K_0;//含有卡尔曼增益的另外一个函数,用于计算最优估计值

       float K_1;//含有卡尔曼增益的函数,用于计算最优估计值的偏差

       float Y_0;

       float Y_1   

       float Rate;//去除偏差后的角速度

       float Pdot[4];//过程协方差矩阵的微分矩阵

       float angle_err;//角度偏量

       float E;//计算的过程量

       static float angle = 0;           //下时刻最优估计值角度

       static float q_bias = 0;        //陀螺仪的偏差                

       static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//过程协方差矩阵

        Rate = gyro_m - q_bias;

       //计算过程协方差矩阵的微分矩阵    

       Pdot[0] = Q_angle - P[0][1] - P[1][0];//??????        

       Pdot[1] = - P[1][1];                        

       Pdot[2] = - P[1][1];                                 

       Pdot[3] = Q_gyro;//??????                        

       angle += Rate * dt; //角速度积分得出角度

       P[0][0] += Pdot[0] * dt; //计算协方差矩阵

       P[0][1] += Pdot[1] * dt;

       P[1][0] += Pdot[2] * dt;

       P[1][1] += Pdot[3] * dt;

       angle_err = incAngle - angle; //计算角度偏差

       E = R_angle + P[0][0];

       K_0 = P[0][0] / E; //计算卡尔曼增益

       K_1 = P[1][0] / E;

       Y_0 = P[0][0];  

       Y_1 = P[0][1];

         P[0][0] -= K_0 * Y_0; //跟新协方差矩阵

       P[0][1] -= K_0 * Y_1;

       P[1][0] -= K_1 * Y_0;

       P[1][1] -= K_1 * Y_1;

       angle += K_0 * angle_err; //给出最优估计值

       q_bias += K_1 * angle_err;//跟新最优估计值偏差

       return angle;

}

2017-12-20 23:20:04 CBQ5789789 阅读数 12346
  • 单片机控制第一个外设-LED灯-第1季第6部分

    本课程是《朱有鹏老师单片机完全学习系列课程》第1季第6个课程,主要讲解LED的工作原理和开发板原理图、实践编程等,通过学习目的是让大家学会给单片机编程控制LED灯,并且为进一步学习其他外设打好基础。

    4000 人正在学习 去看看 朱有鹏

因为毕业设计做室内定位,需要对RSSI值进滤波,采用卡尔曼滤波。网上看了很多卡尔曼滤波的文章,为了加深自己的印象。所以打算结合网上的资料加上自己理解方式写一篇文章(如果不对还请您帮忙纠正)。
在理解卡尔曼之前我们先讲个故事,比如现在我们要测一个房间的温度,那么我们需要一个温度计。假设市场上买两种温度计,一种测得温度非常准没有误差。另一种只能大概测量。当然测得准的温度计,比较贵,另外一个比较便宜。如果你是土豪那么你可以买比较贵的就是没有误差的,那么你就跟卡尔曼没有故事了。
现在假设你是一个中等屌丝,你可以卖得起两个破的温度计,现在两个温度计测量得到两个值,我们假设他一个是23度,一个是25度,那么实际应该是多少度呢。如果你对他们求平均(即权值是0.5,0.5),你会得到24度左右。再说的明确一点呢24±1度,应为这个是你猜的,所以你不能肯定,那么这个1度就是你的不确定度。可能你是天蝎座的,比较偏心,不想然两个温度计都雨露均沾,我们假设你比较相信前一个温度计,不相信另一个,这里我们取权值为(0.7,0.3),那么你得到的温度值就是0.7*23+0.3*25=23.6度,那么不确定度可能是1(感觉自己偏心的有道理,猜的比较准),可能是6度(这个6是随便写的,猜的不准误差比较大)。根据上面这个问题我们发现
1.不确定我们猜的准不准,换句话来说就是不确定度不知道多少
2.权值该怎么取才合理

下面我们来看一下卡尔曼先生是怎么做的。还是原来的例子。假设现在你是一个终极大屌丝,只买的起一个温度计了。但是你有一个特异功能,猜的很准就是你睁开眼就知道今天是几度,该穿大裤衩还是长袖(相当于你有一个温度模型)。之前讲的不确定度,这个太low了,我们换一种高级的说法,卡尔曼先生用协方差来替代不确定度。我们现在再来一个假设。

假设有21:19分房间的温度是23度,协方差是9(原来是讲不确定度)。
好,时间很快现在21:20分了,这一分钟时间内没有阳光直射,没有开着空调,所以你发挥你的神力,猜测现在温度和之前差不多也是23度(也就是现在的温度等于之前的温度),协方差是16(你对你神力的信任值,随便写的)。
现在我们有一个相同的温度值,但是有两个协方差(9和16)。现在把两个协方差整合在一起。这样可能会更准一点。该怎么整合呢,卡尔曼先生告诉我们直接加起来就行了。通过神力及卡尔曼先生的方法,我们得到了23度,协方差25。
这个结果基本是靠猜的,我们称他为预测阶段。

接下来别忘了我们还有一个温度计,从温度计上,读出的温度值是25度,协方差是16(随便写的)。现在又有两个值了。
1. 23度,协方差25(预测阶段得到了)
2. 25度,协方差16(温度计读出)
我们该如何加权呢?(0.5,0.5)又或者是(0.3,0.7)。卡尔曼先生告诉我们可以用两个协方差得到权值(25/(25+16))^0.5=0.78,通常又把这个结果称为卡尔曼增益。有了这个结果就可以计算出21:20分的温度值了,0.78*25+(1-0.78)*23=24.56度,那么21:20分的协方差是多少呢,卡尔曼先生同样给了一个公式(1-0.78)*25(预测协方差)=5.5
有了21:20分的温度值,及协方差,我们就可以重复上面的步骤,进行迭代。
对上面过程的特殊数据,一般化。
1.假设21:19分温度为23及协方差为9(分别用X(k-1|k-1)和P(k-1|k-1)来表示)
2.猜测21:20分温度与21:19分温度相同,协方差为16(分别用X(k|k-1)和Q来表示)
3.计算温度,及协方差(结果用P(k|k-1)表示)可以得到下面两个公式
X(k|k-1)=X(k-1|k-1);
P(k|k-1)=P(k-1|k-1)+Q;
4. 读温度计的值为25,猜测协方差为16(分别用Z(k)和R表示)
5.计算权值,即卡尔曼增益(用K(k)来表示)
6.计算21:20分温度及协方差(分别用X(k|k)和P(k|k)表示)可以得到下面三个公式
K(k)=P(k|k-1)/(P(k|k-1)+R)
X(k|k)=X(k|k-1)+K(k)*(Z(k)-X(k|k-1))(展开与上面计算过程一样)
P(k|k)=(1-K(k))*P(k|k-1)
这五个公式就是,对卡尔曼五个核心公式进行简化,有了这五个公式可以很方便的在单片机上实现

float kalmanFilter_A(float inData) 
{
  static float prevData=0; 
  static float p=10, q=0.0001, r=0.005, kGain=0;
    p = p+q; 
    kGain = p/(p+r);

    inData = prevData+(kGain*(inData-prevData)); 
    p = (1-kGain)*p;

    prevData = inData;

    return inData; 
}

其中p的初值可以随便取,但是不能为0(为0的话卡尔曼滤波器就认为已经是最优滤波器了)
q,r的值需要我们试出来,讲白了就是(买的破温度计有多破,以及你的超人力有多强)

r参数调整滤波后的曲线与实测曲线的相近程度,r越小越接近。

q参数调滤波后的曲线平滑程度,q越小越平滑。
具体过程可以以看一下这篇博客

2019-10-23 15:50:53 qq153471503 阅读数 113
  • 单片机控制第一个外设-LED灯-第1季第6部分

    本课程是《朱有鹏老师单片机完全学习系列课程》第1季第6个课程,主要讲解LED的工作原理和开发板原理图、实践编程等,通过学习目的是让大家学会给单片机编程控制LED灯,并且为进一步学习其他外设打好基础。

    4000 人正在学习 去看看 朱有鹏

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


基本讲解在这里: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…

2019-05-20 16:19:05 u011344545 阅读数 618
  • 单片机控制第一个外设-LED灯-第1季第6部分

    本课程是《朱有鹏老师单片机完全学习系列课程》第1季第6个课程,主要讲解LED的工作原理和开发板原理图、实践编程等,通过学习目的是让大家学会给单片机编程控制LED灯,并且为进一步学习其他外设打好基础。

    4000 人正在学习 去看看 朱有鹏

github:https://github.com/MichaelBeechan
CSDN:https://blog.csdn.net/u011344545

============================================================

其他相关卡尔曼滤波博客:
单片机开发 | 基于51单片机实现MPU6050的卡尔曼滤波算法(代码类)
单片机开发 | 基于MPU6050的卡尔曼滤波算法(代码类)

============================================================

1、前言(Introduction)

卡尔曼滤波是一种利用实测数据估计系统状态的算法。它最初是由匈牙利工程师鲁道夫·卡尔曼(Rudolf Kalman)开发的,卡尔曼滤波器算法就是以他的名字命名的。滤波器的算法分为两步:第一步预测系统状态,第二步利用噪声测量对系统状态估计进行细化

现在有几个变种的卡尔曼滤波器。这些滤波器广泛用于依赖于估计的应用,包括计算机视觉、制导和导航系统、计量经济学和信号处理。

2、制导、导航和控制(Guidance, Navigation, and Control)

卡尔曼滤波器广泛应用于GNC系统中,如传感器融合,通过融合GPS和惯性测量单元(IMU)的测量来合成位置和速度信号。滤波器通常用于估计无法测量的信号的值,比如飞机发动机涡轮中的温度,在这种情况下,任何温度传感器都会失灵。在线性二次高斯控制中,滤波器还与线性二次调节器(LQR)补偿器一起使用。

下面利用卡尔曼滤波估计飞机的位置
在这里插入图片描述

3、计算机视觉(Computer Vision)

在计算机视觉应用中,卡尔曼滤波器用于跟踪目标,预测目标的未来位置,考虑目标检测位置中的噪声,并帮助将多个目标与其对应的轨迹关联起来。

下面是跟踪球的轨迹。卡尔曼滤波的输出用红圈表示,目标检测用黑色表示。注意当球被遮挡,没有检测到;过滤器被用来预测它的位置
在这里插入图片描述

4、基本卡尔曼滤波算法(Kalman Filtering)

研究说明了卡尔曼滤波器的设计与仿真。同时考虑了稳态和时变卡尔曼滤波器。

4.1 Plant Dynamics
考虑一个离散对象在输入 u[n] 上存在加性高斯噪声 w[n] :

x[n+1] = Ax[n] + B(u[n] + w[n])
y[n] = Cx[n].

此外,设输出 y[n] 的噪声测量为vyv[n] ,v[n]表示测量噪声:

yv[n] = y[n] + v[n].

The following matrices represent the dynamics of the plant.

A = [1.1269   -0.4940    0.1129;
     1.0000         0         0;
          0    1.0000         0];

B = [-0.3832;
      0.5919;
      0.5191];

C = [1 0 0];

4.2 离散卡尔曼滤波器
给出了该问题的稳态卡尔曼滤波方程
在这里插入图片描述
上述方程中,
在这里插入图片描述表示x[n]的估计,根据过去的测量结果给出在这里插入图片描述
在这里插入图片描述是基于上次测量在这里插入图片描述的更新估计
根据当前估计在这里插入图片描述存在:
在这里插入图片描述
在给定噪声协方差的情况下,选择创新增益M来最小化估计误差的稳态协方差:
在这里插入图片描述
您可以将时间和测量更新方程组合成一个状态空间模型,即卡尔曼滤波器:
在这里插入图片描述
滤波器产生在这里插入图片描述记滤波器状态为:在这里插入图片描述

参考文献:
Grimble, M.J., Robust Industrial Control: Optimal Design Approach for Polynomial Systems, Prentice Hall, 1994, p. 261 and pp. 443-456.

2015-06-07 19:20:35 cfxzy 阅读数 1205
  • 单片机控制第一个外设-LED灯-第1季第6部分

    本课程是《朱有鹏老师单片机完全学习系列课程》第1季第6个课程,主要讲解LED的工作原理和开发板原理图、实践编程等,通过学习目的是让大家学会给单片机编程控制LED灯,并且为进一步学习其他外设打好基础。

    4000 人正在学习 去看看 朱有鹏

http://bbs.elecfans.com/jishu_484132_1_1.html

逛博客看到的一篇讲解卡尔曼滤波的文章,发现程序十分简洁易懂,而且使用效果不错,是一个学习的典范,在此分享了!

#ifndef _KALMAN_H_
#define _KALMAN_H_


extern  KalmanGain;//  卡尔曼增益
extern  EstimateCovariance;//估计协方差
extern  MeasureCovariance;//测量协方差
extern  EstimateValue;//估计值
extern void KalmanFilterInit(void);
extern      KalmanFilter(   Measure);
#endif






#include "config.h"
#include "math.h"


  KalmanGain;//  卡尔曼增益
  EstimateCovariance;//估计协方差
  MeasureCovariance;//测量协方差
  EstimateValue;//估计值


void KalmanFilterInit(void);


extern    float  KalmanFilter(float   Measure);




void KalmanFilterInit(void)
{
EstimateValue=0;


EstimateCovariance=0.1;
MeasureCovariance=0.02;




}


KalmanFilter(   Measure)
{


//计算卡尔曼增益
KalmanGain=EstimateCovariance*sqrt(1/(EstimateCovariance*EstimateCovariance+MeasureCovariance*MeasureCovariance));


//计算本次滤波估计值
EstimateValue=EstimateValue+KalmanGain*(Measure-EstimateValue);
//更新估计协方差


EstimateCovariance=sqrt(1-KalmanGain)*EstimateCovariance;
//更新测量方差
MeasureCovariance=sqrt(1-KalmanGain)*MeasureCovariance;
//返回估计值
return EstimateValue;
}




卡尔曼滤波1

阅读数 6694

没有更多推荐了,返回首页