结合C++代码介绍Kalmanfilter卡尔曼滤波的5个公式

代码来自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