数据卡尔曼滤波处理

float max[5]={110},min[5]={20};
float data2[30],datal[5],datall[5];
float sum=0;
u8 n=0,ok=0,i=0,j=0;
float c1[5]={110,110,110,110,110};//假设我们的感觉是110 卡尔曼滤波
float c[5]={110,110,110,110,110};//假设我们的感觉是110 卡尔曼滤波
float P1[5]={10,10,10,10,10};//初始参数，带来一个初始的kg
float P2[5]={10,10,10,10,10};//初始参数，带来一个初始的kg
float Q=1e-6;//人体感觉的方差，原始
float R=1e-2;//传感器数据的方差
float kg[5]={0};//卡尔曼增益
float c2[5]={0};

if(ok==0){
if(n<30){
n++;
if(h[2]<130&&h[2]>95)
data2[n]=h[2];
}
else{
for(i=0;i<30;i++)
{sum+=data2[i];}
for(i=0;i<5;i++)
{max[i]=sum/30;}
ok=1;
}
}
for(i=0;i<5;i++){
if(h[i]>max[i])
{max[i]=h[i];}
}
for(i=0;i<5;i++){
P2[i]=P1[i]+Q;
c2[i]=c1[i];//假设读数不变
kg[i]=P2[i]/(P2[i]+R);
c[i]=c2[i]+kg[i]*(max[i]-c2[i]);//b（m）代表在第m秒的传感器读数，c（m）是经过卡尔曼滤波后得到的读数
P1[i]=(1-kg[i])*P2[i];//c（m）是经过卡尔曼滤波后得到的读数
c1[i]=c[i];
max[i]=c[i];
}
for(i=0;i<5;i++){
h[i]=110*h[i]/(max[i]-min[i]);
}

for(j=0;j<5;j++){
datall[j]=datal[j];
datal[j]=h[j];
h[j]=(datal[j]+datall[j]+h[j])/3.0;
}