背景介紹：
MPU6050 姿態(tài)解算系列目的是讓初學(xué)者容易入門，所以表述上一直淡化有難度的數(shù)學(xué)語言而改用“文字語言的數(shù)學(xué)形式”。
Kalman 濾波涉及的數(shù)學(xué)內(nèi)容比較多，網(wǎng)上有很多講卡爾曼濾波原理的文章，數(shù)學(xué)功底欠佳的讀者可能看不懂。Sugar 本篇的目標(biāo)是：
用更通俗易懂的方式表達(dá)“線性 Kalman 濾波的效果”，讓讀者不需要太深的數(shù)學(xué)功底就能知道線性 Kalman 濾波的作用。
思維鋪墊Kalman 濾波用狀態(tài)空間來描述研究對象，是一種時域下的濾波方法。在正式進(jìn)入算法講解之前，先羅列幾個 Sugar 覺得比較重要的點(diǎn)來做預(yù)先的思維導(dǎo)向：
1、在 Kalman 濾波方法中，系統(tǒng)的觀測噪聲是狀態(tài)估計所依賴的重要信息，并不是需要濾除的對象。
2、因?yàn)?Kalman 濾波是在時域下的遞推形式，所以計算量較小，容易實(shí)現(xiàn)。
3、Kalman 濾波器可以看成：是狀態(tài)變量在由觀測生成的線性空間上的投影。 Kalman 濾波器與投影“投影”英文是“projection”。
上面“思維鋪墊”的第 3 點(diǎn)，我們可以這樣寫： 基于 k 個觀測值對 j 時刻狀態(tài)的估計 = proj( j 時刻系統(tǒng)的真實(shí)狀態(tài) | k 個觀測值)
表示：j 時刻的狀態(tài)估計是j 時刻真實(shí)狀態(tài)在由某一角度上的 k 個觀測值生成的線性空間上的投影。讀著比較繞，下面 Sugar 舉個實(shí)際的例子，在這個例子里，注意找到狀態(tài)估計、真實(shí)狀態(tài)，并理解什么是某一角度，例： Sugar 在屋里寫推文，聽到陽臺窗子的風(fēng)聲。
Sugar 想：“外面正在刮多大的風(fēng)呢？”
在不打開窗子的情況下：Sugar 看窗外柳樹的擺動幅度，
默默觀察一會兒(默記了 k 個幅度值)，
估計現(xiàn)在有 3 級風(fēng)力。
狀態(tài)估計是現(xiàn)在有 3 級風(fēng)力；
真實(shí)狀態(tài)是在刮某一級風(fēng)，并不確切知道風(fēng)力；
某一角度是在屋里不開窗靠眼力估計風(fēng)力的角度。
現(xiàn)在，我們可以把 Kalman 濾波的投影解釋寫的稍微數(shù)學(xué)一點(diǎn)：
X'(j|k) = proj( X(j) | Y(1), Y(2), ..., Y(k) )
X'(j|k) 表示 基于 k 個觀測值對 j 時刻狀態(tài)的估計；
X(j) 表示 j 時刻的真實(shí)狀態(tài)，顯然這個是并不知道的；
Y(1), Y(2), ..., Y(k) 表示 k 個觀測生成的線性空間
延伸：對于 j=k, j>k, j<k, 分別稱X'(j|k)為 Kalman 濾波器、預(yù)報器和平滑器。濾波器一般是對當(dāng)前狀態(tài)噪聲的處理。 狀態(tài)描述1、狀態(tài)轉(zhuǎn)移
A稱作狀態(tài)轉(zhuǎn)移矩陣，這個公式的意思是：系統(tǒng)當(dāng)前的狀態(tài)能夠通過用狀態(tài)轉(zhuǎn)移矩陣對上一時刻的狀態(tài)進(jìn)行轉(zhuǎn)移，再加上系統(tǒng)當(dāng)前的實(shí)際噪聲來表示。
2、狀態(tài)觀測
z(t)表示對當(dāng)前系統(tǒng)的觀測值，這個公式是說：系統(tǒng)當(dāng)前的觀測值就是系統(tǒng)當(dāng)前的狀態(tài)與當(dāng)前觀測噪聲v(t)之和 。C是觀測矩陣，表示：對哪個目標(biāo)進(jìn)行了觀測。
3、系統(tǒng)狀態(tài)受額外影響的情況
(1) 什么叫額外影響
以自由落體運(yùn)動的觀測為例，我們的目標(biāo)觀測量是“位置”和“速度”，而因?yàn)槭艿街亓铀俣鹊挠绊?，我們目?biāo)觀測量“位置”和“速度”都會受到影響。這里重力加速度g就叫額外影響。
(2) B*u(t)是什么
以位移和速度為例，第 1 點(diǎn)中的狀態(tài)轉(zhuǎn)移方程能描述“均速”運(yùn)動，若速度是變化的，那么就要把速度變化對系統(tǒng)狀態(tài)的影響考慮進(jìn)來。但在狀態(tài)轉(zhuǎn)移矩陣 A 中只有“速度對位移的影響”，如果要引入加速度對速度和位移的影響，就要加上這個 B*u(t)。
上面引出的比較突然，Sugar 在 github 上準(zhǔn)備了三個線性 Kalman 應(yīng)用的 MATLAB 實(shí)時腳本用來解釋上面的內(nèi)容，在公眾號回復(fù) Kalman 獲得。因?yàn)檫@里的內(nèi)容很多，放推文里太占篇幅，所以就突然出現(xiàn)一下，點(diǎn)到為止了。
在做線性 Kalman 濾波的時候，因?yàn)槲覀儾⒉恢老到y(tǒng)的真實(shí)狀態(tài)，所以我們會用系統(tǒng)當(dāng)前的狀態(tài)估計來代替系統(tǒng)當(dāng)前的狀態(tài)。既然不知道真實(shí)狀態(tài)，自然也不可能知道系統(tǒng)當(dāng)前的實(shí)際噪聲w(t)，那么這個噪聲該怎么辦呢？答案是：忽略掉。因?yàn)樵肼晫ο到y(tǒng)的影響小，解決濾波問題才有意義。如果噪聲已經(jīng)大到足以覆蓋系統(tǒng)狀態(tài)的話，我們要考慮換個角度觀測（就像在嘈雜的環(huán)境下打電話，如果聽不清，最好的方法是換個清靜的地方或者帶上耳機(jī)，只是沖著電話大聲喊對于聽力是沒有幫助的）。
說到這里，用于實(shí)際編程的公式就變成了：
線性 Kalman 濾波方程在理解上面的內(nèi)容后，我們只要記住下面 5 個方程就可以去設(shè)計線性 Kalman 濾波器了。
1、時間更新方程(2個)
Q是系統(tǒng)噪聲方差，我們在第一個公式里忽略了系統(tǒng)噪聲，這個系統(tǒng)噪聲方差就變成了需要手調(diào)的值，其實(shí)際意義就是：給手動干預(yù)留下的一個入口。
P是協(xié)方差，描述估計值的準(zhǔn)確程度。在線性 Kalman 濾波里P不是我們關(guān)心的重點(diǎn)，只要按公式計算就行了。
注意：B*u(t)是可選項，并不是一定要加上，加與不加要看是否有額外影響目標(biāo)觀測量的因素。
2、狀態(tài)更新方程(3個)
R是觀測的方差，這個我們可以通過觀測值的統(tǒng)計數(shù)據(jù)獲得。
K是Kalman 增益，其作用就是調(diào)整濾波程度的大小。
從這個公式我們來重新理解一下“思維鋪墊”里的第 1 個點(diǎn)：Kalman 濾波是在利用系統(tǒng)當(dāng)前的觀測噪聲。
最后一步是協(xié)方差P的迭代。
線性 Kalman 濾波用于姿態(tài)解算以姿態(tài)解算為例，一步一步按上面的規(guī)則設(shè)計一個線性 Kalman 濾波器。
一、確定目標(biāo)量 姿態(tài)解算我們關(guān)注的是：橫滾角、橫滾角速度、俯仰角和俯仰角速度。把這些目標(biāo)量裝到 state_estimate 列向量里，如下： state_estimate = [橫滾角; 橫滾角速度; 俯仰角; 俯仰角速度];
為了便于表示，我們把文字改成英文： state_estimate = [phi; phi_rate; theta; theta_rate];
二、確定狀態(tài)轉(zhuǎn)移矩陣 A = [ 1, dt, 0, 0][ 0, 1, 0, 0][ 0, 0, 1, dt][ 0, 0, 0, 1]
為什么 A 長這樣的？我們看下 MATLAB 怎么說：
>> A*state_estimate ans = phi + dt*phi_rate phi_rate theta + dt*theta_rate theta_rate
這樣容易看出：結(jié)果仍然符合我們第一步確定的目標(biāo)量的排列方式。 三、確定要不要有 B*u(t) 在姿態(tài)解算中，我們的目標(biāo)量是“橫滾角”、“俯仰角”、“橫滾角速度”和“俯仰角速度”，這些都可以通過 MPU6050 觀測到，沒有影響目標(biāo)量的額外因素。因此，在本篇的姿態(tài)解算中不需要 B*u(t)。 四、系統(tǒng)噪聲 Q 系統(tǒng)噪聲 Q 是留出的手動調(diào)節(jié)參數(shù)，看下 Sugar 在 MATLAB 里是怎么把這個調(diào)節(jié)接口留出來的：
var_acc_init = 1e-2;var_gyro_init = 6.75e0;
var_acc = [var_acc_init, var_acc_init]';var_gyro = [var_gyro_init, var_gyro_init]';
Q = eye(4);Q(1,1) = Q(1,1) * var_acc(1);Q(2,2) = Q(2,2) * var_gyro(1);Q(3,3) = Q(3,3) * var_acc(2);Q(4,4) = Q(4,4) * var_gyro(2);
五、觀測噪聲 R 觀測噪聲要隨采樣計算，Sugar 在 MATLAB 里取的是每一時刻的前 10 個采樣值的方差給觀測噪聲 R，MATLAB 的程序是這樣的：
cnt = 10;if i>cnt var_acc(:,i) = [var(phi_acc(size(phi_acc,1)-cnt:end)), var(theta_acc(size(theta_acc,1)-cnt:end))]'; var_gyro(:,i) = [var(phi_rate_gyro(size(phi_rate_gyro,1)-cnt:end)), var(theta_rate_gyro(size(theta_rate_gyro,1)-cnt:end))]';else var_acc(:,i) = [var(phi_acc), var(theta_acc)]'; var_gyro(:,i) = [var(phi_rate_gyro), var(theta_rate_gyro)]';end
R = eye(4);R(1,1) = R(1,1) * var_acc(1,i);R(2,2) = R(2,2) * var_gyro(1,i);R(3,3) = R(3,3) * var_acc(2,i);R(4,4) = R(4,4) * var_gyro(2,i);
六、應(yīng)用線性 Kalam 的 5 個公式開始遞推 這一步?jīng)]太多要說的，直接用上面介紹過的 5 個公式就行了，如下：
state_estimate = A * state_estimate;P = A * P * A' + Q;K = P * C' * inv(R + C * P * C');state_estimate = state_estimate + K * (measurement - C * state_estimate);P = (eye(4) - K * C) * P;
效果視頻
|
