由于工作需要，最近需要用到一种数字滤波算法，经过与高手的交流，研究了高斯滤波算法和卡尔曼滤波算法；后来还是选择卡尔曼算法；因为他的思想比较简单，也是比较稳定的；他可以不断校正，不断更新；但是有一定的弊端；比如我们在测量之前还要测量一些数据作为他的Q因子（即过程激励方差）和R因子；
上面是他基本思想，我们可以把他简化为下面几个步骤：
/*
* Kalman Filter Algorithm
* ----------------------------------------------------------
* Time Update Equation
* [1] Xk = X[k-1];
* [2] Pk = P[k-1] + Q;
* PS : Q Factor is As Incentive Noise Variance;
* ----------------------------------------------------------
* Measurement Update Equation
* [3] Kk = Pk / (Pk + R);
* [4] X[k] = Xk + Kk * (Zm - Xk);
* [5] P[k] = (1 - Kk) * Pk;
* PS : R Factor is As The process covariance noise;
* Zm : Measure Value or True Value
* ----------------------------------------------------------
* Update Next X[*] And P[*]
* [6] X[k-1] = X[k];
* [7] P[k-1] = P[k];
* ----------------------------------------------------------
* Author : -Rain
* E-mail : ieczw@qq.com
*/
#define __DEBUGM__
//#define __DEBUG__
#ifdef __DEBUGM__
#include <stdio.h>
#endif
#define QVALUE 0.005 // Incentive noise variance
#define RVALUE 15.0 // The process covariance noise
double KalmanUpdata(double TrueValue)
{
char k = 1; // Just As a Scalar
static double X[2],Xk;
static double P[2],Pk;
static double Kk;
/*
* Time Update
*/
Xk = X[k-1];
Pk = P[k-1] + QVALUE;
#ifdef __DEBUG__
printf( "Time Update : Q = %f\n"
"Xk = %f\n"
"Pk = %f\n", QVALUE,Xk,Pk);
#endif
/*
* Measurement Update Equation
*/
Kk = Pk / (Pk + RVALUE);
X[k] = Xk + Kk * (TrueValue - Xk);
P[k] = (1 - Kk) * Pk;
#ifdef __DEBUG__
printf( "Measurement Update : R = %f\n"
"Kk = %f\n"
"X[k] = %f\n"
"P[k] = %f\n"
,RVALUE,Kk,X[k],P[k]);
#endif
/*
* Update Next X[*] And P[*]
*/
X[k-1] = X[k];
P[k-1] = P[k];
return X[k];
}
#ifdef __DEBUGM__
#define INC 0.01
#define IDEAL 0.0
int main(void)
{
/*
* Rand Value : rand_value : -15 ~ 15 ;
* average value of rand_value EQU 0;
* Noise Value -15 ~ 15; So We Should Set R = 15.0
* Increment : inn += INC; INC = 0.01
* Noise Value -0.005 ~ 0.005; So Set Q = 0.005
*/
int i = 0;
double rand_value;
double inn = 0.0;
for(i=0; i<100000; i++){
/*
* Increment : INC = 0.01
* Noise Value : 0.001*(5 - rand()%11) = -0.005 ~ 0.005
*/
inn += INC + 0.001*(5 - rand()%11);
/*
* Ideal Value : IDEAL = 0.0
* Noise Value : 15.0 - rand() % 31 = -15 ~ 15
*/
rand_value = IDEAL + inn + 15.0 - rand() % 31;
printf("True Value : %f\n",rand_value);
printf("Kalman Update Value : %f\n",KalmanUpdata(rand_value));
}
return 0;
}
#endif
原文：http://blog.csdn.net/ieczw/article/details/21627771