最近在改進之前做的視覺定點演算法，以前只有一個位置環，現在準備再串一級速度環，但是解算出無人機的平移速度還是頗為頭疼的，網上的資料很少，需要我們自己動腦去解決這個問題。
首先要測水平速度，傳統的方法是GPS，我所設計的無人機的應用場景中的GPS訊號雖然有，但是並不那麼穩定，其次GPS的精度也達不到我所需的視覺引導的要求，因為GPS的位置環並沒有反饋出我需要的靶標物的位置。
其次如果單純用加速度計進行積分得到速度，那麼你會發現靜置一段時間後，解算的速度值不知道飄到哪裡去了，這種漂移太大，完全不能夠滿足我們的設計要求。
但是GPS融合加速度計來測量速度卻是有很多的應用例子，於是我們可以將視覺解算出的位置資訊和加計資訊進行融合。

最開始我使用的是互補濾波演算法對影象資料和加計資料進行了一次融合，效果差強人意，這裡只放出程式碼，一階和二階的差別不大，只放出一階的互補濾波程式碼：

    Para_pos.filter_vx = Para_pos.filter_vx_coeff * Para_pos.v_x\
        + (1 - Para_pos.filter_vx_coeff)*(Para_pos.filter_vx + ax*dt);
    Para_pos.filter_vy = Para_pos.filter_vy_coeff * Para_pos.v_y\
        + (1 - Para_pos
 
.filter_vy_coeff)*(Para_pos.filter_vy + ay*dt);

兩個方向：融合出x和y方向的速度，調節filter_vx_coeff,filter_vy_coeff這兩個係數即可，表示對加積分的速度和影象解算的速度的信任程度，可以看出程式碼很簡單，也確實是這樣，不過效果還是有的，像在兩輪平衡車上，如果對角度解算的要求不是那麼高的話，這個演算法就足以滿足平衡車的直立要求了。

下面進入正題：卡爾曼濾波對兩個感測器的資料進行融合
卡爾曼濾波主要是運用5個方程（推導就不給出了）：兩個預測方程和3個更新方程。
預測方程：

更新方程：

卡爾曼濾波器在線性系統中產生最優估計，因此，感測器或者系統必須是(接近)線性系統才能被運用卡爾曼濾波。因為僅僅依靠上一次系統狀態和定義系統狀態被修正概率的方差矩陣，所以卡爾曼濾波器不需要很長的系統狀態歷史來經行濾波。這點在可確保系統的高實時性要求。卡爾曼濾波有兩類方程:預測方程和更新方程。預測方程根據之前的狀態和控制量預測當前狀態；更新方程表示相信感測器資料多些還是相信總體估計值多些（由卡爾曼增益Kg決定）。濾波器的大致工作原理為：根據預測方程預測當前狀態並用更新方程檢測預測結果，該過程一直在重複更新當前狀態。預測方程和更新方程的關係如圖3-3所示。

下面這段是擷取的別人對卡爾曼濾波在陀螺儀與加速度計融合上的應用對卡爾曼濾波做的一個解釋，照著文章裡的解釋和一些矩陣論（矩陣乘法，協方差矩陣等）的知識結合上面的5個方程，就能看懂整個推導過程，最後程式設計實現即可。

卡爾曼濾波程式碼主要根據式（3-2）~（3-6）5個公式來寫的。在此，以Y軸角速度在時間上的積分為預測值的控制量，即U(k)=Gyro_Y；俯仰角（pitch=arctan(ACCEL_X/ACCEL_Z)）作為觀測值，即Z(k)=pitch。因為需要同時得到Y軸的角速度和pitch角，所以進行卡爾曼濾波時需要估計兩個值，一個是pitch角，另一個是陀螺儀漂移Q_bias。根據式（3-2）建立角度測量模型方程：（原諒我要直接截圖了，公式編輯起來實在麻煩）

最後貼出我的卡爾曼融合的程式碼，只貼了x方向的

void Kalman_Filter(float vx, float acc_x，float dt)
{

        filter_vx.v_acc+=(acc_x-filter_vx.q_bias) * dt;
        filter_vx.v_err = vx - filter_vx.v_acc;
        filter_vx.Pdot[0]=filter_vx.Q_v - filter_vx.P[0][1] - filter_vx.P[1][0];
        filter_vx.Pdot[1]=- filter_vx.P[1][1];
        filter_vx.Pdot[2]=- filter_vx.P[1][1];
        filter_vx.Pdot[3]=filter_vx.Q_acc;
        filter_vx.P[0][0] += filter_vx.Pdot[0] * dt;
        filter_vx.P[0][1] += filter_vx.Pdot[1] * dt;
        filter_vx.P[1][0] += filter_vx.Pdot[2] * dt;
        filter_vx.P[1][1] += filter_vx.Pdot[3] * dt;
        filter_vx.PCt_0 = filter_vx.C_0 * filter_vx.P[0][0];
        filter_vx.PCt_1 = filter_vx.C_0 * filter_vx.P[1][0];
        filter_vx.E = filter_vx.R_angle + filter_vx.C_0 * filter_vx.PCt_0;
        filter_vx.K_0 = filter_vx.PCt_0 / filter_vx.E;
        filter_vx.K_1 = filter_vx.PCt_1 / filter_vx.E;
        filter_vx.t_0 = filter_vx.PCt_0;
        filter_vx.t_1 = filter_vx.C_0 * filter_vx.P[0][1];
        filter_vx.P[0][0] -= filter_vx.K_0 * filter_vx.t_0;
        filter_vx.P[0][1] -= filter_vx.K_0 * filter_vx.t_1;
        filter_vx.P[1][0] -= filter_vx.K_1 * filter_vx.t_0;
        filter_vx.P[1][1] -= filter_vx.K_1 * filter_vx.t_1;
        filter_vx.v_acc += filter_vx.K_0 * filter_vx.v_err; 
        filter_vx.q_bias += filter_vx.K_1 * filter_vx.v_err;
        Para_pos.filter_vx = filter_vx.v_acc;
}

最後看看融合的效果，最上面的是影象解算出的位置波形，中間毛刺和噪聲大的是影象差分算出的速度波形。還有綠色的波形是融合後的速度波形。

可以看出，融合後的速度波形基本可以反映出影象的運動方向和運動速度，不像單獨的影象速度波形噪聲那麼大，效果非常不錯。

最後再對資料做個低通濾波即可使用了，對於速度環的控制上來講，這個精度已經足夠使用了，如果還想提高解算的精度，要進一步提高影象部分的演算法，目前的影象演算法上，還是會有丟目標的情況發生，進一步提升之後，效果應該會更好

另外說一句，現在很多光流模組(大多是抄襲的PixFlow)也可以和加速計進行融合，效果應該會更好。我改動過pixflow的程式碼和引數，對於旋轉光流的消除還是有些疑惑，線性的補償效果都不怎麼好，在我的視覺系統中本來打算引入光流模組的，但是後來想到在我應用場景中可能不太適合，也就沒有使用了，有光流模組使用經驗的朋友歡迎與我交流，怎麼消除像旋轉光流這樣的噪聲，有時間我也會寫一寫我對光流模組的一些研究。