平衡小車卡爾曼濾波算法
這次的平衡車，使用到了卡爾曼濾波，下面談?wù)勈褂眯牡?/p>
我們是利用角速度傳感器和加速度傳感器測(cè)量得到角度和角速度，但是由于車子是運(yùn)動(dòng)的，我們利用加速度得到的角度并不完全正確，由于噪聲干擾，我們對(duì)角速度傳感器的測(cè)量值也存在懷疑。于是我們就要進(jìn)行濾波，通過兩個(gè)傳感器數(shù)值上的相互關(guān)系來得到我們想要的結(jié)果。我們使用卡爾曼濾波器連接這兩個(gè)測(cè)量值。
首先開感性的理解一下卡爾曼，引用網(wǎng)上(百度百科)的經(jīng)典解釋：
在介紹他的5條公式之前，先讓我們來根據(jù)下面的例子一步一步的探索。
假 設(shè)我們要研究的對(duì)象是一個(gè)房間的溫度。根據(jù)你的經(jīng)驗(yàn)判斷，這個(gè)房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度(假設(shè)我們用一分鐘來做時(shí) 間單位)。假設(shè)你對(duì)你的經(jīng)驗(yàn)不是100%的相信，可能會(huì)有上下偏差幾度。我們把這些偏差看成是高斯白噪聲(White Gaussian Noise)，也就是這些偏差跟前后時(shí)間是沒有關(guān)系的而且符合高斯分配(Gaussian Distribution)。另外，我們?cè)诜块g里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的，測(cè)量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。
好了，現(xiàn)在對(duì)于某一分鐘我們有兩個(gè)有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗(yàn)的預(yù)測(cè)值(系統(tǒng)的預(yù)測(cè)值)和溫度計(jì)的值(測(cè)量值)。下面我們要用這兩個(gè)值結(jié)合他們各自的噪聲來估算出房間的實(shí)際溫度值。
假 如我們要估算k時(shí)刻的是實(shí)際溫度值。首先你要根據(jù)k-1時(shí)刻的溫度值，來預(yù)測(cè)k時(shí)刻的溫度。因?yàn)槟阆嘈艤囟仁呛愣ǖ，所以你?huì)得到k時(shí)刻的溫度預(yù)測(cè)值是跟 k-1時(shí)刻一樣的，假設(shè)是23度，同時(shí)該值的高斯噪聲的偏差是5度(5是這樣得到的：如果k-1時(shí)刻估算出的最優(yōu)溫度值的偏差是3，你對(duì)自己預(yù)測(cè)的不確定 度是4度，他們平方相加再開方，就是5)。然后，你從溫度計(jì)那里得到了k時(shí)刻的溫度值，假設(shè)是25度，同時(shí)該值的偏差是4度。
由于我們用 于估算k時(shí)刻的實(shí)際溫度有兩個(gè)溫度值，分別是23度和25度。究竟實(shí)際溫度是多少呢?相信自己還是相信溫度計(jì)呢?究竟相信誰(shuí)多一點(diǎn)，我們可以用他們的 covariance來判斷。因?yàn)镵g^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時(shí)刻的實(shí)際溫度值是：23+0.78* (25-23)=24.56度？梢钥闯，因?yàn)闇囟扔?jì)的covariance比較小(比較相信溫度計(jì))，所以估算出的最優(yōu)溫度值偏向溫度計(jì)的值。
現(xiàn) 在我們已經(jīng)得到k時(shí)刻的最優(yōu)溫度值了，下一步就是要進(jìn)入k+1時(shí)刻，進(jìn)行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒看到什么自回歸的東西出現(xiàn)。對(duì)了，在進(jìn)入 k+1時(shí)刻之前，我們還要算出k時(shí)刻那個(gè)最優(yōu)值(24.56度)的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。這里的5就是上面的k時(shí) 刻你預(yù)測(cè)的那個(gè)23度溫度值的偏差，得出的2.35就是進(jìn)入k+1時(shí)刻以后k時(shí)刻估算出的最優(yōu)溫度值的偏差(對(duì)應(yīng)于上面的3)。
就是這樣，卡爾曼濾波器就不斷的把covariance遞歸，從而估算出最優(yōu)的溫度值。他運(yùn)行的很快，而且它只保留了上一時(shí)刻的covariance。上面的Kg，就是卡爾曼增益(Kalman Gain)。他可以隨不同的時(shí)刻而改變他自己的值，是不是很神奇!
然后看看我們的代碼，代碼來自網(wǎng)絡(luò)，使用的是ouravr某大牛的代碼
#include "Kalman.h"
float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;
//注意：dt的取值為kalman濾波器采樣時(shí)間;
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;//先驗(yàn)估計(jì)
Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先驗(yàn)估計(jì)誤差協(xié)方差的微分
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;// Pk- 先驗(yàn)估計(jì)誤差協(xié)方差微分的積分 = 先驗(yàn)估計(jì)誤差協(xié)方差
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
angle_err = angle_m - angle;//zk-先驗(yàn)估計(jì)
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;//后驗(yàn)估計(jì)誤差協(xié)方差
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;//后驗(yàn)估計(jì)
q_bias += K_1 * angle_err;//后驗(yàn)估計(jì)
angle_dot = gyro_m-q_bias;//輸出值(后驗(yàn)估計(jì))的微分 = 角速度
}
我們一個(gè)個(gè)語(yǔ)句進(jìn)行解釋
angle+=(gyro_m-q_bias) * dt
首先我們要利用系統(tǒng)的過程模型，來預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài)：
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)!!這可不是我們要的結(jié)果
然后是
Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先驗(yàn)估計(jì)誤差協(xié)方差的微分
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;// Pk- 先驗(yàn)估計(jì)誤差協(xié)方差微分的積分 = 先驗(yàn)估計(jì)誤差協(xié)方差
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
這8句一起進(jìn)行解釋
到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于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)
積分后協(xié)方差就算出來了，同樣注意也是P(k|k-1)。具體怎么算~~~好吧我承認(rèn)我線代沒有學(xué)好~~~算了好久。。。。
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是測(cè)量系統(tǒng)的矩陣，為(1
1)
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;//后驗(yàn)估計(jì)誤差協(xié)方差
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要另卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的covariance：
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
這個(gè)很好理解了~~I是單位矩陣不多說鳥~~
angle += K_0 * angle_err;//后驗(yàn)估計(jì)
q_bias += K_1 * angle_err;//后驗(yàn)估計(jì)
angle_dot = gyro_m-q_bias;//輸出值(后驗(yàn)估計(jì))的微分 = 角速度
現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
觀察一下K_1計(jì)算過程，再聯(lián)系到協(xié)方差矩陣的性質(zhì)就可以知道為什么角速度偏差量用P[1][0]算了~~
至于STM32上跑的速度，72M下這段代碼執(zhí)行時(shí)間在0.5毫秒內(nèi)，速度不是問題~~
編輯：admin 最后修改時(shí)間：2018-05-18
