uble)RAND_MAX) - 0.5)； //随机噪声

#include “stdio.h”
#include “stdlib.h”
#include “math.h”

double frand()
{
return 2*((rand()/(double)RAND_MAX) - 0.5); //随机噪声
}

int main()
{
printf(“Hello world!\n”);
float x_last=0;
float p_last=0.02;
float Q = 0.018;
float R = 0.542;
float kg;
float x_mid;
float x_now;
float p_mid;
float p_now;
float z_real = 0.56;
float z_measure;
float sumerror_kalman = 0;
float sumerror_measure = 0;
int i;
x_last = z_real + frand() * 0.03;
x_mid = x_last;

printf("index,   x_last,   kg,     Real,   z_measure,    Kalman, real-meas,  real-now \n" );  //显示
for(i = 0;i < 200; 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为kalman filter，R为噪声
    z_measure = z_real + frand() * 0.03;//测量值
    x_now     = x_mid + kg * (z_measure - x_mid);//估计出的最优值
    p_now     = (1 - kg) * p_mid;//最优值对应的covariance


    if (i == 100)
    {
        z_real = 1;
    }

    printf(" %d,    %6.3f,   %6.3f, %6.3f,  %6.3f,       %6.3f  ,%6.3f, %6.3f \n",i, x_last, kg, z_real, z_measure,x_now ,fabs(z_real-z_measure), fabs(z_real - x_now));

    //printf("Real     : %6.3f Mesaured :%6.3f [diff:%.3f],Kalman : %6.3f [diff:%.3f]  \n",z_real, z_measure,fabs(z_real-z_measure),x_now,fabs(z_real - x_now) );  //显示
    //printf("Mesaured : %6.3f [diff:%.3f]\n",z_measure,fabs(z_real-z_measure));  //显示测量值以及真值与测量值之间的误差
   // printf("Kalman   : %6.3f [diff:%.3f]\n",x_now,fabs(z_real - x_now));  //显示kalman估计值以及真值和卡尔曼估计值的误差

    sumerror_kalman += fabs(z_real  - x_now);  //kalman估计值的累积误差
    sumerror_measure += fabs(z_real - z_measure);  //真值与测量值的累积误差

    p_last = p_now;  //更新covariance值
    x_last = x_now;  //更新系统状态值

}

printf("total meas e     : %f\n",sumerror_measure);  //输出测量累积误差
printf("total kalman e   : %f\n",sumerror_kalman);   //输出kalman累积误差

printf("kalman p: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100)); //卡尔曼误差所占比例

return 0;

}