编程实现插值滤波器（扩展卡尔曼滤波器跟踪曲线）

//下面是用c语言编写的代码(将其复制到自己的main.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(void)

{

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) ;

}

,