代碼來自github:https://github.com/TKJElectronics/KalmanFilter
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). Contact information ------------------- Kristian Lauszus, TKJ Electronics Web : http://www.tkjelectronics.com e-mail : kristianl@tkjelectronics.com */ #include "Kalman.h" Kalman::Kalman() { /* We will set the variables like so, these can also be tuned by the user */ Q_angle = 0.001f; Q_bias = 0.003f; R_measure = 0.03f; angle = 0.0f; // Reset the angle
bias = 0.0f; // Reset bias
P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
P[0][1] = 0.0f; P[1][0] = 0.0f; P[1][1] = 0.0f; }; // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
float Kalman::getAngle(float newAngle, float newRate, float dt) { // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
// Modified by Kristian Lauszus // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
// Discrete Kalman filter time update equations - Time Update ("Predict") // Update xhat - Project the state ahead
/* Step 1 */ rate = newRate - bias; angle += dt * rate; // Update estimation error covariance - Project the error covariance ahead
/* Step 2 */ P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1]; P[1][0] -= dt * P[1][1]; P[1][1] += Q_bias * dt; // Discrete Kalman filter measurement update equations - Measurement Update ("Correct") // Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
float S = P[0][0] + R_measure; // Estimate error
/* Step 5 */
float K[2]; // Kalman gain - This is a 2x1 vector
K[0] = P[0][0] / S; K[1] = P[1][0] / S; // Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
float y = newAngle - angle; // Angle difference
/* Step 6 */ angle += K[0] * y; bias += K[1] * y; // Calculate estimation error covariance - Update the error covariance
/* Step 7 */
float P00_temp = P[0][0]; float P01_temp = P[0][1]; P[0][0] -= K[0] * P00_temp; P[0][1] -= K[0] * P01_temp; P[1][0] -= K[1] * P00_temp; P[1][1] -= K[1] * P01_temp; return angle; };
主要貼出了Kalmanfilter相關部分代碼，下面將結合Kalmanfilter的5公式解析上面代碼。
Step 1.KalmanFilter第一個公式根據測試值評估:
X(k) = AX(k-1) + Bu(k-1) + w(k-1)
建立該模型是測量角度的，但是陀螺儀在沒有轉向時也有電壓輸出也就是零偏（bias）
建立矩陣：
angle = angle + (newRate – bias) * dt 推算現角度 = 原角度 + 角速度×時間間隔
bias = bias 不會受到陀螺儀轉向的影響認為上一刻的值
轉換成矩陣描述：
其中：
=> X(k)
=> A
=> B
newRate => u(k)
w(k) 不確定誤差沒有考慮
對應代碼：
rate = newRate – bias; //bias 角速度零偏
angle += dt * rate; //推算現角度 = 原角度 + 角速度×時間間隔
Step 2.KalmanFilter第二個公式協方差矩陣預測:
P(k) = AP(k-1)A’ + Q
=> P(k)
=> A
=> A’
Q是 的協方差矩陣為
因為零偏噪聲和角度噪聲是相互獨立的，所以cov(angle,bias)=cov(bias,angle)=0
cov(x,x)即方差所以Q 轉換成
做矩陣運算后得到：
對應代碼：
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);//由於dt較小所以沒有考慮p11×dt×dt
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
Question:根據公式推導p00中的 Q_angle和p11中的 Q_bias沒有乘以dt，為什么乘以dt?
Step 3.KalmanFilter第三個公式計算卡爾曼增益:
K(k)= P(k-1)H’/(HP(k-1)H’ + R)
其中H是測量值和實際值的轉換關系是一個放縮關系，
相對於 矩陣，angle和測量值比較沒有進行放縮所以是 1，bias沒有測量值所以是0
所以對應的
帶入得到：
=> K(k)
R 觀測噪聲誤差是常數
做矩陣運算后得到：
K(angle) = p00/(p00 + R)
K(bias) = p10/(p00 + R)
對應代碼：
float S = P[0][0] + R_measure; // Estimate error
float K[2]; // Kalman gain - This is a 2x1 vector
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
Step 4.KalmanFilter第四個公式根據卡爾曼增益進行修正:
X(k)= X(k-1)+Kg(k)(Z(k)- HX(k-1))
其中Z(k)是測量矩陣
做矩陣運算后得到：
對應代碼：
float y = newAngle - angle; // Angle difference
angle += K[0] * y;
bias += K[1] * y;
Step 5.KalmanFilter第五個公式更新協方差矩陣:
P(k)=（1-Kg(k)H）P(k-1)
做矩陣運算后得到：
對應代碼：
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
上文主要介紹了kalmanfilter 5個公式的使用，
關於kalmanfilter原理可以參考：
https://blog.csdn.net/u010720661/article/details/63253509
該大神寫的特別好。
如果使用該文請指明出處：https://www.cnblogs.com/yueyangtze/p/9503835.html