扩展卡尔曼滤波器c语言编程

以上是简单扩展卡尔曼滤波器的重要公式，要严格应用公式进行进行计算。

//下面是用c语言编写的代码(将其复制到自己的main.c中就可以编译运行)
#include <stdio.h>
#include <math.h>
#include <windows.h>
#include <time.h>
#include <stdlib.h>
#define     N       50
#define     Q       0.5
#define     R       2
void gotoxy(int x,int y);
void setColor(int color);
int main()
{
    float  x[N];      //状态方程
    float  xekf[N];   //滤波值
    float  y[N];      //观测方程
    float  faik[N];   //状态矩阵
    float  Zn[N];     //观测预测
    float  Hn[N];     //观测矩阵
    float  Kn[N];     //卡尔曼增益
    float  Pn[N];     //协方差预测
    float  P0[N];     //协方差更新
    float  error[N];  //误差
    float  temp[N];   //中间变量
    float  Vk[N];     //过程噪声
    float  Wk[N];     //观测噪声
    int i,n;
    FILE *fp = NULL;
    fp = fopen("D:\\test.csv","w") ;//在你的D盘建立一个test.csv文件
    //产生随机观测和随机过程噪声随机数
    for(i=0;i<N;i++)
    {
        srand((unsigned)time(NULL));
        Vk[i]=rand()%1;
        Wk[i]=rand()%2;
    }
    x[0]=0.1; //初始值
    y[0]=(x[0] * x[0]) / 20 + Vk[0];//初始值
    setColor(6);
    printf("------------------------------------------------------------------------------------\n");
    printf("真实值\t\t\t观测值\t\t\t滤波值\t\t\t误差\n");
    printf("------------------------------------------------------------------------------------\n");
    for(i=1;i<N;i++)
    {
        x[i]=0.5 * x[i-1] + (2.5 * x[i-1]) / (1 + x[i-1] * x[i-1]) + 8 * cos(1.2 * i) +  sqrt(Q) * Wk[i-1];
        y[i]=(x[i] * x[i]) / 20 + sqrt(R) * Vk[i];
        temp[i]=x[i];
        printf("%f\t\t%f\n",x[i],y[i]);//先打印这两个值
        printf("------------------------------------------------------------------------------------\n");
    }
    //EKF滤波算法
    xekf[0]=0.1;
    P0[0]=1.0;
    //状态预测
    for(n=1;n<N;n++)
    {
    //状态预测
        xekf[n]=0.5 * xekf[n-1] + (2.5 * xekf[n-1]) / (1 + xekf[n-1] * xekf[n-1]) + 8 * cos(1.2 * n);
    //观测预测
        Zn[n]=(x[n] * x[n]) / 20;
    //状态转移矩阵
        faik[n]=0.5 + 2.5 * (1 - x[n] * x[n]) / ((1 + x[n] * x[n]) * (1 + x[n] * x[n])) ;
    //求观测矩阵
        Hn[n]=xekf[n] / 10;
    //求协方差预测
        Pn[n]=faik[n] * P0[n-1] * faik[n] + Q;
    //求卡尔曼增益
        Kn[n]=Pn[n] * Hn[n] * ( Hn[n] * Pn[n] * Hn[n] + R);
    //状态更新
        xekf[n]=xekf[n] + Kn[n] * (y[n] - Zn[n]);
        gotoxy(48,2 * n + 1);
        printf("%f\n",xekf[n]);//打印滤波值
    //协方差更新
        P0[n]=(1.0 - Kn[n] * Hn[n]) * Pn[n];
        error[n]=fabs(xekf[n] - temp[n]);
        gotoxy(70,2 * n + 1);
        printf("%f\n",error[n]);//打印误差
         if(n==1)
        {
            fprintf(fp,"%s,%s,%s,%s\n","真实值","观测值","滤波值","误差");
        }
        else
        {
            fprintf(fp,"%f,%f,%f,%f\n",temp[n],y[n],xekf[n],error[n]);
        }
    }
        fclose(fp);
    for(i=0;i<2 * N;i++)
    {
        gotoxy(15,i);
        printf("|");
        gotoxy(39,i);
        printf("|");
        gotoxy(62,i);
        printf("|");
        gotoxy(83,i);
        printf("|");
    }
    printf("\n\n");
    return 0;
}
//光标位置函数
void gotoxy(int x,int y)
{
    COORD pos;
    pos.X=x;
    pos.Y=y;
    SetConsoleCursorPosition(GetStdHandle(STD_OUTPUT_HANDLE),pos);
}
//文字背景颜色函数
void setColor(int color)
{
    SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE), color) ;
}

运行结果