卡尔曼滤波算法代码总结(20150311)

/***************************************************************** **********/
/* kalman.c */
/* 1-D Kalman filter Algorithm, using an inclinometer and gyro */
/* Author: Rich Chi Ooi */
/* Version: 1.0 */
/* Date:30.05.2003 */
/* Adapted from Trammel Hudson() */
/* ------------------------------- */
/* Compilation procedure: */
/* Linux */
/* gcc68 -c XXXXXX.c (to create object file) */
/* gcc68 -o XXXXXX.hex XXXXXX.o ppwa.o */
/*Upload data : */
/* ul */
/***************************************************************** **********/
/* In this version: */
/***************************************************************** **********/
/* This is a free software; you can redistribute it and/or modify */
/* it under the terms of the GNU General Public License as published */
/* by the Free Software Foundation; either version 2 of the License, */
/* or (at your option) any later version. */
/* */
/* this code is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */ /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the */
/* GNU General Public License for more details. */
/* */
/* You should have received a copy of the GNU General Public License */
/* along with Autopilot; if not, write to the Free Software */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, */
/* MA 02111-1307 USA */
/***************************************************************** **********/
#include <math.h>
#include "eyebot.h"
/*
* The state is updated with gyro rate measurement every 20ms
* change this value if you update at a different rate.
*/
static const float dt = 0.02;
/*
* The covariance matrix.This is updated at every time step to
* determine how well the sensors are tracking the actual state.
*/
static float P[2][2] = {{ 1, 0 },
{ 0, 1 }};
/*
* Our two states, the angle and the gyro bias.As a byproduct of computing * the angle, we also have an unbiased angular rate available.These are
* read-only to the user of the module.
*/
float angle;
float q_bias;
float rate;
/*
* The R represents the measurement covariance noise.R=E[vvT]
* In this case,it is a 1x1 matrix that says that we expect
* 0.1 rad jitter from the inclinometer.
* for a 1x1 matrix in this case v = 0.1
*/
static const float R_angle = 0.001 ;
/*
* Q is a 2x2 matrix that represents the process covariance noise.
* In this case, it indicates how much we trust the inclinometer
* relative to the gyros.
*/
static const float Q_angle = 0.001;
static const float Q_gyro = 0.0015;
/*
* state_update is called every dt with a biased gyro measurement
* by the user of the module. It updates the current angle and
* rate estimate.
*
* The pitch gyro measurement should be scaled into real units, but
* does not need any bias removal. The filter will track the bias.
*
* A = [ 0 -1 ]
* [ 0 0 ]
*/
void stateUpdate(const float q_m){
float q;
float Pdot[4];
/* Unbias our gyro */
q = q_m - q_bias;//当前角速度：测量值-估计值
/*
* Compute the derivative of the covariance matrix
* (equation 22-1)
* Pdot = A*P + P*A' + Q
*
*/
Pdot[0] = Q_angle - P[0][1] - P[1][0]; /* 0,0 */
Pdot[1] = - P[1][1]; /* 0,1 */
Pdot[2] = - P[1][1]; /* 1,0 */
Pdot[3] = Q_gyro; /* 1,1 */
/* Store our unbias gyro estimate */
rate = q;
/*
* Update our angle estimate
* angle += angle_dot * dt
* += (gyro - gyro_bias) * dt
* += q * dt
*/
angle += q * dt;//角速度积分累加到估计角度
/* Update the covariance matrix */
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
}
/*
* kalman_update is called by a user of the module when a new
* inclinoometer measurement is available.
*
* This does not need to be called every time step, but can be if
* the accelerometer data are available at the same rate as the
* rate gyro measurement.
*
* H = [ 1 0 ]
*
* because the angle measurement directly corresponds to the angle * estimate and the angle measurement has no relation to the gyro * bias.
*/
void kalmanUpdate(const float incAngle)
{
/* Compute our measured angle and the error in our estimate */ float angle_m = incAngle;
float angle_err = angle_m - angle;//1.12 zk-H*xk_dot
/*
* h_0 shows how the state measurement directly relates to
* the state estimate.
*
* H = [h_0 h_1]
*
* The h_1 shows that the state measurement does not relate
* to the gyro bias estimate. We don't actually use this, so
* we comment it out.
*/
float h_0 = 1;
/* const float h_1 = 0; */
/*
* Precompute PH' as the term is used twice
* Note that H[0,1] = h_1 is zero, so that term is not not computed */
const float PHt_0 = h_0*P[0][0]; /* + h_1*P[0][1] = 0*/
const float PHt_1 = h_0*P[1][0]; /* + h_1*P[1][1] = 0*/
/*
* Compute the error estimate:
* (equation 21-1)
*
* E = H P H' + R
*/
float E = R_angle +(h_0 * PHt_0);
/*
* Compute the Kalman filter gains:
* (equation 21-2)
*
* K = P H' inv(E)
*/
float K_0 = PHt_0 / E;
float K_1 = PHt_1 / E;
/*
* Update covariance matrix:
* (equation 21-3)
*
* P = P - K H P
* Let
* Y = H P
*/
float Y_0 = PHt_0; /*h_0 * P[0][0]*/
float Y_1 = h_0 * P[0][1];
P[0][0] -= K_0 * Y_0;
P[0][1] -= K_0 * Y_1;
P[1][0] -= K_1 * Y_0;
P[1][1] -= K_1 * Y_1;
/*
* Update our state estimate:
*
* Xnew = X + K * error
*
* err is a measurement of the difference in the measured state
* and the estimate state. In our case, it is just the difference
* between the inclinometer measured angle and the estimated angle.
*/
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
}
//现在智能小车上用的卡尔曼滤波算法。

由于做平衡小车，然后对那段滤波算法很疑惑，然后网上讲的又比较少，我看了一段时间的书。

这是小弟的对这段卡尔曼滤波程序的一点理解，因为基础薄弱（大二），有错的请多多包涵。

先上程序，这是抄的不知道谁的代码。

抱歉了。

不过这程序好像都写的差不多
void Kalman_Filter(float Gyro,float Accel)
{
Angle+=(Gyro - Q_bias) * dt;
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; /
Pdot[1]= - PP[1][1];
Pdot[2]= - PP[1][1];/
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle;
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
Gyro_x = Gyro - Q_bias;
}
首先是卡尔曼滤波的5个方程
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)//先验估计
P(k|k-1)=A P(k-1|k-1) A ’+Q ……… (2)//协方差矩阵的预测
Kg(k)= P(k|k-1) H ’ / (H P(k|k-1) H ’ + R) ……… (3)//计算卡尔曼增益
X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通过卡尔曼增益进行修正
P(k|k)=（I-Kg(k) H ）P(k|k-1) ……… (5)//跟新协方差阵
5个式子比较抽象，现在直接用实例来说
—，对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d dt θω=⨯，角度微分等于时间的微分乘以角速度。

但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。

由此我们得到了当前角度的预测值 Angle
Angle=Angle+(Gyro - Q_bias) * dt;
其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔。

float dt=0.005; 这是程序中的定义
同时 Q_bias 也是一个变化的量。

但是就预测来说认为现在的漂移跟上一时刻是相同的即
Q_bias=Q_bias
将两个式子写成矩阵的形式
1_01_0
Angle
dt Angle
dt
Q bias Q bia o s Gyr -=+
得到上式，这个式子对应于卡尔曼滤波的第一个式子
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)//先验估计
二，这里是卡尔曼滤波的第二个式子
接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值的噪声，（所谓噪声就是数据的方差值）
P(k|k-1)=A P(k-1|k-1) A ’+Q
因为漂移噪声还有角度噪声是相互独立的，则cov(Angle,Q_bias)=0；cov(Q_bias,Angle)
=0
又由性质可知cov （x ，x ）=D （x ）即方差，所以得到的矩阵如下
程序中的定义如下float Q_angle=0.001;
float Q_gyro=0.003;
接着是这一部分A P(k-1|k-1) A ’,其中的（P （k-1）|P(k-1)）为上一时刻的预测方差阵 卡尔曼滤波的目标就是要让这个预测方差阵最小。

则计算A P(k-1|k-1) A ’+Q （就是个矩阵乘法和加法，算算吧）结果如下
2.(dt)d 很小为了计算简便忽略不计。

于是得到
a,b,c,d 分别和矩阵的P[0][0],P[0][1],P[1][0],P[1][1]
计算过程转化为如下程序，代换即可
Pdot[0]=Q_angle - PP[0][1] - PP[1][0];
Pdot[1]= - PP[1][1];
Pdot[2]= - PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
三，这里是卡尔曼滤波的第三个式子
Kg(k)= P(k|k-1) H ’ / (H P(k|k-1) H ’ + R) ……… (3)//计算卡尔曼增益
即计算卡尔曼增益，这是个二维向量设为0
1k k
P(K|K-1)+R ，这里又有一个常数R ，程序中的定义如下
float R_angle=0.5;
这个指的是角度测量噪声值，则式子的分母=P[0][0]+R_angle 即程序中的
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0;
分子 [0][0]
[1][0]P P
于是求出
1K K K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
四，用误差还有卡尔曼增益来修正
X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通过卡尔曼增益进行修正
这个矩阵带进去就行了Z（k)=Accel.....注意这个是加速度计算出来的角度
Angle_err = Accel - Angle;
对应程序如下
Angle += K_0 * Angle_err;
Q_bias += K_1 * Angle_err;
同时为了PID控制还有下次的使用把角速度算出来了
Gyro_x = Gyro - Q_bias;
五，最后一步对矩阵P进行更新，因为下一次滤波时要用到
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)//跟预测方差阵
这个很简单，矩阵带进去算就行了
六，总结
卡尔曼滤波一共只需要给很少的初始值量，
float Q_angle=0.001;
float Q_gyro=0.003;
还有float R_angle=0.5;
以及系统的初始量angle还有Q_bias
还有预测误差矩阵P，程序里给的是0（数组）
理论上由于卡尔曼滤波是迭代的算法，当时间充分长以后。

滤波估值将与初始值的选取无关。

但是实际上并不是如此，比如测量方差值一直在变化。