找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 22392|回复: 8
收起左侧

关于stm32进行卡尔曼滤波制作平衡车的一些心得和理解

[复制链接]
ID:80436 发表于 2015-5-20 14:11 | 显示全部楼层 |阅读模式

关于卡尔曼滤波也是刚刚开始研究,很荣幸能和大家一起讨论学习,新浪微博:@颠覆的饕餮 希望能和大家一起学习~~
转载请声明出处并且保持文章完整性谢谢
这次的平衡车,使用到了卡尔曼滤波,下面谈谈使用心得
我们是利用角速度传感器和加速度传感器测量得到角度和角速度,但是由于车子是运动的,我们利用加速度得到的角度并不完全正确,由于噪声干扰,我们对角速度传感器的测量值也存在怀疑。于是我们就要进行滤波,通过两个传感器数值上的相互关系来得到我们想要的结果。我们使用卡尔曼滤波器连接这两个测量值。
首先开感性的理解一下卡尔曼,引用网上(百度百科)的经典解释:
在介绍他的5条公式之前,先让我们来根据下面的例子一步一步的探索。      
假 设我们要研究的对象是一个房间的温度。根据你的经验判断,这个房间的温度是恒定的,也就是下一分钟的温度等于现在这一分钟的温度(假设我们用一分钟来做时 间单位)。假设你对你的经验不是100%的相信,可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声(White Gaussian Noise),也就是这些偏差跟前后时间是没有关系的而且符合高斯分配(Gaussian Distribution)。另外,我们在房间里放一个温度计,但是这个温度计也不准确的,测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。      
好了,现在对于某一分钟我们有两个有关于该房间的温度值:你根据经验的预测值(系统的预测值)和温度计的值(测量值)。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。      
假 如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值,来预测k时刻的温度。因为你相信温度是恒定的,所以你会得到k时刻的温度预测值是跟 k-1时刻一样的,假设是23度,同时该值的高斯噪声的偏差是5度(5是这样得到的:如果k-1时刻估算出的最优温度值的偏差是3,你对自己预测的不确定 度是4度,他们平方相加再开方,就是5)。然后,你从温度计那里得到了k时刻的温度值,假设是25度,同时该值的偏差是4度。      
由于我们用 于估算k时刻的实际温度有两个温度值,分别是23度和25度。究竟实际温度是多少呢?相信自己还是相信温度计呢?究竟相信谁多一点,我们可以用他们的 covariance来判断。因为Kg^2=5^2/(5^2+4^2),所以Kg=0.78,我们可以估算出k时刻的实际温度值是:23+0.78* (25-23)=24.56度。可以看出,因为温度计的covariance比较小(比较相信温度计),所以估算出的最优温度值偏向温度计的值。      
现 在我们已经得到k时刻的最优温度值了,下一步就是要进入k+1时刻,进行新的最优估算。到现在为止,好像还没看到什么自回归的东西出现。对了,在进入 k+1时刻之前,我们还要算出k时刻那个最优值(24.56度)的偏差。算法如下:((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时 刻你预测的那个23度温度值的偏差,得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差(对应于上面的3)。      
就是这样,卡尔曼滤波器就不断的把covariance递归,从而估算出最优的温度值。他运行的很快,而且它只保留了上一时刻的covariance。上面的Kg,就是卡尔曼增益(Kalman Gain)。他可以随不同的时刻而改变他自己的值,是不是很神奇!
然后看看我们的代码,代码来自网络,使用的是ouravr某大牛的代码
#include "Kalman.h"
float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;   
            //注意:dt的取值为kalman滤波器采样时间;     
float P[2][2] = {     
                            { 1, 0 },     
                            { 0, 1 }     
                        };     
float Pdot[4] ={0,0,0,0};
const char C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;   
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m)            //gyro_m:gyro_measure   
{     
    angle+=(gyro_m-q_bias) * dt;//先验估计     
    Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分     
    Pdot[1]=- P[1][1];     
    Pdot[2]=- P[1][1];     
    Pdot[3]=Q_gyro;     
    P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差     
    P[0][1] += Pdot[1] * dt;     
    P[1][0] += Pdot[2] * dt;     
    P[1][1] += Pdot[3] * dt;     
    angle_err = angle_m - angle;//zk-先验估计     
    PCt_0 = C_0 * P[0][0];     
    PCt_1 = C_0 * P[1][0];     
    E = R_angle + C_0 * PCt_0;     
    K_0 = PCt_0 / E;//Kk     
    K_1 = PCt_1 / E;     
    t_0 = PCt_0;     
    t_1 = C_0 * P[0][1];
    P[0][0] -= K_0 * t_0;//后验估计误差协方差   
    P[0][1] -= K_0 * t_1;     
    P[1][0] -= K_1 * t_0;     
    P[1][1] -= K_1 * t_1;     
    angle    += K_0 * angle_err;//后验估计     
    q_bias    += K_1 * angle_err;//后验估计     
    angle_dot = gyro_m-q_bias;//输出值(后验估计)的微分 = 角速度     
}
我们一个个语句进行解释

angle+=(gyro_m-q_bias) * dt
首先我们要利用系统的过程模型,来预测下一状态的系统。假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
我们的矩阵X为:
(angle
    gyro)
我们的矩阵A为:
( 1     1
      0     1)
要注意的是我们得到的是X(k|k-1)!!这可不是我们要的结果


然后是
Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分   
Pdot[1]=- P[1][1];     
Pdot[2]=- P[1][1];     
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差  
P[0][1] += Pdot[1] * dt;  
P[1][0] += Pdot[2] * dt;  
P[1][1] += Pdot[3] * dt;     
这8句一起进行解释
到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的covariance还没更新。我们用P表示covariance:
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
Pdot是P的微分。
我们的Q是
(Q_angle 0
    0            Q_gyro)
积分后协方差就算出来了,同样注意也是P(k|k-1)。具体怎么算~~~好吧我承认我线代没有学好~~~算了好久。。。。
angle_err = angle_m - angle;//这句好像没有必要说~~


接下来算卡尔曼增益:
    PCt_0 = C_0 * P[0][0];   
    PCt_1 = C_0 * P[1][0];   
    E = R_angle + C_0 * PCt_0;   
    K_0 = PCt_0 / E;   
    K_1 = PCt_1 / E;
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
H是测量系统的矩阵,为(1
                                                1)



    t_0 = PCt_0;   
    t_1 = C_0 * P[0][1];
    P[0][0] -= K_0 * t_0;//后验估计误差协方差   
    P[0][1] -= K_0 * t_1;   
    P[1][0] -= K_1 * t_0;   
    P[1][1] -= K_1 * t_1;
到现在为止,我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的covariance:
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
这个很好理解了~~I是单位矩阵不多说鸟~~

    angle    += K_0 * angle_err;//后验估计   
    q_bias    += K_1 * angle_err;//后验估计   
    angle_dot = gyro_m-q_bias;//输出值(后验估计)的微分 = 角速度
现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值。结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k):
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
观察一下K_1计算过程,再联系到协方差矩阵的性质就可以知道为什么角速度偏差量用P[1][0]算了~~
over~~写得好累~~算得也好累~~有空再重新排一下版~~


回复

使用道具 举报

ID:121757 发表于 2016-5-18 17:00 | 显示全部楼层
请问楼主Q_angle=0.001, Q_gyro=0.003, R_angle=0.5这三个值来的依据是什么,测量偏差可以用标准差,但是过程误差Q依据什么,看来两晚上资料了,求楼主指点
回复

使用道具 举报

ID:136370 发表于 2016-8-9 18:33 | 显示全部楼层
我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度,角速度和倾角,我发现当我快速的改变板子的倾角的时候,比如快速变化10度,Angle(卡尔曼滤波后的倾角)瞬时变化非常快,可能会瞬间变化几十度然后回到正常角度,而当我缓慢变化10度的时候,Angle变化是正常线性变化到10度,在这两种变化中,Angle_ax(从MPU6050读取的值经过处理后的陀螺仪的Y轴数据)的变化一直都是线性正常的,并且Angle的值特别接近Angle_ax的值
问题:1,我快速改变板子倾角时Angle的变化正常吗?
      2,Angle正常变化的时候是应该与Angle_ax的值相近吗?

现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现中间的角度增大


*************读取数据********************
//定义MPU6050内部地址
#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00
#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s
#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG     0x37
#define INT_ENABLE      0x38
#define INT_STATUS      0x3A    //只读
#define        ACCEL_XOUT_H        0x3B
#define        ACCEL_XOUT_L        0x3C
#define        ACCEL_YOUT_H        0x3D
#define        ACCEL_YOUT_L        0x3E
#define        ACCEL_ZOUT_H        0x3F
#define        ACCEL_ZOUT_L        0x40
#define        TEMP_OUT_H                0x41
#define        TEMP_OUT_L                0x42
#define        GYRO_XOUT_H    0x43
#define        GYRO_XOUT_L                0x44       
#define        GYRO_YOUT_H        0x45
#define        GYRO_YOUT_L                0x46
#define        GYRO_ZOUT_H        0x47
#define        GYRO_ZOUT_L                0x48

//读取寄存器原生数据
           
        MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);
        MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);
        MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
        MPU6050_Raw_Data.Temp =    (buf[6]<<8 | buf[7]);  
        MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
        MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
        MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
      
      
        //将原生数据转换为实际值,计算公式跟寄存器的配置有关
        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
        MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
              MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   
    }
   


//******卡尔曼参数************
               
const float Q_angle=0.001;  
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01;                          //dt为kalman滤波器采样时间;
const char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/*****************卡尔曼滤波**************************************************/
void Kalman_Filter(float Accel,float Gyro)               
{
        Angle+=(Gyro - Q_bias) * dt; //先验估计
       
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

        Pdot[1]= -PP[1][1];
        Pdot[2]= -PP[1][1];
        Pdot[3]=Q_gyro;
       
        PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
        PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
               
        Angle_err = Accel - Angle;        //zk-先验估计
       
        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[1][0];
       
        E = R_angle + C_0 * PCt_0;
       
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
       
        t_0 = PCt_0;
        t_1 = C_0 * PP[0][1];

        PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
               
        Angle        += K_0 * Angle_err;         //后验估计
        Q_bias        += K_1 * Angle_err;         //后验估计
        Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度

}

******************倾角计算*****************
void Angle_Calculate(void)
{

/****************************加速度****************************************/
       
        Accel_x  =  MPU6050_Real_Data.Accel_X;          //读取X轴加速度
        Angle_ax = Accel_x*1.2*180/3.14;     //弧度转换为度

/****************************角速度****************************************/
       
         Gyro_y = MPU6050_Real_Data.Gyro_Y;             
时间dt,所以此处不用积分
/***************************卡尔曼融合*************************************/
        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
       

回复

使用道具 举报

ID:219618 发表于 2017-7-14 11:18 | 显示全部楼层
楼主,你的矩阵A和P是怎么确定的
回复

使用道具 举报

ID:219618 发表于 2017-7-15 10:27 | 显示全部楼层
求协方差的时候,为什么要先求微分再求积分得到协方差,还有协方差的微分形式怎么来的
回复

使用道具 举报

ID:221096 发表于 2017-7-25 17:44 | 显示全部楼层
先MARK
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表