Kalman 滤波

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

double frand()
{
    return 2*((rand ()/(double)RAND_MAX)-0.5);
}
/**卡尔曼滤波算法
 * 1、有两个值：一个预测值；另一个为测量值
 * 2、预测值P(k) = P(k-1)+R(k)     // P(k-1)为上一时刻的预测值，R为预测值的方差；
 * 3、测量值T(k) = t(k)+Q(k)       // t(k)为k时刻的真实值，Q为测量误差
 * 3、R(k)^2 = U^2 + D(k-1)^2     // U为预测值得不可信度； D(k-1)为k-1次最优预估值P(k-1)的偏差
 * 4、Kg(k)^2 = R(k)^2 / (R(k)^2 + Q(k)^2)
 * 5、D(k)^2 = （1-Kg(k-1)）*R(k-1)^2
 *
 * 最优值X(k) = P(k) + Kg * (P(k)-T(k))
 */

int main(int argc, char *argv[])
{
    float x_last =0;
    float p_last =0.2;
    float R = 0.542;
    float Q = 0;//0.018;
    float kg;
    float x_mid;
    float p_mid;
    float x_now;
    float p_now;
    float z_real = 0.56;
    float z_measure;
    float sumerror_kalman=0;
    float sumerror_measure=0;
    int i=0;
    x_last = z_real+frand()*0.5;
    x_mid = x_last;

    FILE *handler = fopen("D:/data.txt","w+");
    printf("handler = %d\n",handler);
    fprintf (handler,"this is a test program \n");

    for(i=0;i<10000;i++)
    {
        x_mid = x_last;                     //x_last = x(k-1|k-1)  ;x_mid = x(k|k-1)
        p_mid = p_last+Q;                   //p_mid  = p(k|k-1)    ;p_last = p(k-1|k-1) Q为噪声
        kg = p_mid/(p_mid+R);               //kg为卡尔曼增益；R为噪声
        z_measure = z_real+frand ()*0.3;    //测量值
        x_now = x_mid+kg*(z_measure-x_mid); //估计出的最优值
        p_now = (1-kg)*p_mid;               //最优值对应的协方差

        fprintf (handler,"%6.3f\t",z_real);
        fprintf (handler,"%6.3f\t",z_measure);
        fprintf (handler,"%6.3f\n",x_now);

        sumerror_kalman += fabs(z_real-x_now); //卡尔曼估计值得累计误差
        sumerror_measure += fabs(z_real - z_measure);   //真实值与测量值的累计误差
        p_last = p_now;     //更新协方差
        x_last = x_now;     //更新状态值
    }

    fprintf (handler,"总体测量误差  ：%f\n",sumerror_measure);
    fprintf (handler,"总体卡尔曼滤波误差：  %f\n",sumerror_kalman);
    fprintf (handler,"卡尔曼误差所占比例：%d%%\n",(int)((sumerror_kalman/sumerror_measure)*100));

    fclose(handler);
}