欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136

標題: 關于stm32進行卡爾曼濾波制作平衡車的一些心得和理解 [打印本頁]

作者: liuqq 時間: 2015-5-20 14:11
標題: 關于stm32進行卡爾曼濾波制作平衡車的一些心得和理解

關于卡爾曼濾波也是剛剛開始研究，很榮幸能和大家一起討論學習，新浪微博：@顛覆的饕餮希望能和大家一起學習~~
轉載請聲明出處并且保持文章完整性謝謝
這次的平衡車，使用到了卡爾曼濾波，下面談談使用心得
我們是利用角速度傳感器和加速度傳感器測量得到角度和角速度，但是由于車子是運動的，我們利用加速度得到的角度并不完全正確，由于噪聲干擾，我們對角速度傳感器的測量值也存在懷疑。于是我們就要進行濾波，通過兩個傳感器數值上的相互關系來得到我們想要的結果。我們使用卡爾曼濾波器連接這兩個測量值。
首先開感性的理解一下卡爾曼，引用網上（百度百科）的經典解釋：

在介紹他的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~~寫得好累~~算得也好累~~有空再重新排一下版~~

作者: maoizi 時間: 2016-5-18 17:00
請問樓主Q_angle=0.001, Q_gyro=0.003, R_angle=0.5這三個值來的依據是什么，測量偏差可以用標準差，但是過程誤差Q依據什么，看來兩晚上資料了，求樓主指點

作者: 瘋狂土豆兒 時間: 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);    //卡爾曼濾波計算傾角

作者: 1503776754 時間: 2017-7-14 11:18
樓主，你的矩陣A和P是怎么確定的

作者: 1503776754 時間: 2017-7-15 10:27
求協方差的時候，為什么要先求微分再求積分得到協方差，還有協方差的微分形式怎么來的

作者: 865 時間: 2017-7-25 17:44
先MARK

歡迎光臨 (http://www.raoushi.com/bbs/)