編程實現插值濾波器?//下面是用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) ;

}

更多精彩资讯请关注tft每日頭條，我们将持续为您更新最新资讯!