用C语言写一个卡尔曼滤波程序

我可以为你提供一个示例：#include <stdio.h>int main(void)
{
// 定义变量
double x[100],y[100],temp[100],est[100],err[100];
int i,j;

// 初始化数据
for(i=0;i<100;i++)
{
x[i]=i;
y[i]=x[i]*x[i];
}

// 初始化Kalman滤波器
double P = 1,Q = 1,R = 0.1,K;
est[0] = 0;
err[0] = 1;

// 卡尔曼滤波
for(i=1;i<100;i++)
{
temp[i] = est[i-1];
P = P+Q;
K = P/(P+R);
est[i] = temp[i]+K*(y[i]-temp[i]);
err[i] = (1-K)*P;
P = err[i];
}

//输出结果
for(j=0;j<100;j++)
{
printf("%f\t%f\n",est[j],y[j]);
}

return 0;
}