卡尔曼滤波算法实现代码.docVIP

卡尔曼滤波算法实现代码 其c语言实现代码如下： #include?stdlib.h??#include?rinv.c牋int爈man(n,m,k,f,q,r,h,y,x,p,g)牋int爊,m,k;m,牋double爁[],q[],r[],h[],y[],x[],p[],g[];牋{爄nt爄,j,kk,ii,l,jj,js;y[牋牋double?e,*a,*b;,js牋牋e=malloc(m*m*sizeof(double));[]牋牋l=m;lo牋牋if?ln)爈=n;*siz牋牋a=malloc(l*l*sizeof(double));[]牋牋b=malloc(l*l*sizeof(double));[]牋牋for?i=0;爄=n-1;爄++)doubl牋牋牋for?j=0;爅=n-1;爅++)ouble)牋牋牋牋or?j{爄i=i*l+j;燼[ii]=0.0;)牋牋牋牋牋for?kk=0;爇k=n-1;爇k++)));[],g牋牋牋牋牋牋a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk];牋牋牋牋}a[ii牋牋for?i=0;爄=n-1;爄++)n+kk]牋牋牋for?j=0;爅=n-1;爅++)+kk]*f牋牋牋牋or?j{爄i=i*n+j;爌[ii]=q[ii];+k牋牋牋牋牋for?kk=0;爇k=n-1;爇k++)f[j*n+kk牋牋牋牋牋牋p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j];牋牋牋牋}p[ii牋牋for?ii=2;爄i=k;爄i++)+kk]*牋牋牋or?{爁or?i=0;爄=n-1;爄++))+k牋牋牋牋for?j=0;爅=m-1;爅++)kk]*a[k牋牋牋牋牋or?j={爅j=i*l+j;燼[jj]=0.0;+)牋牋牋牋牋牋for?kk=0;爇k=n-1;爇k++)[kk*l+j];牋牋牋牋牋牋牋a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk];牋牋牋牋牋}a[jj]牋牋牋牋for?i=0;爄=m-1;爄++)+kk]*h[牋牋牋牋for?j=0;爅=m-1;爅++)+kk]*h[牋牋牋牋牋or?j={爅j=i*m+j;爀[jj]=r[jj];+牋牋牋牋牋牋for?kk=0;爇k=n-1;爇k++)h[j*n+kk]牋牋牋牋牋牋牋e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];牋牋牋牋牋}e[jj]牋牋牋牋js=rinv(e,m);+h[i牋牋牋牋if?js==0)?m);+h牋牋牋牋牋f?js={爁ree(e);爁ree(a);爁ree(b);爎eturn(js);};牋牋牋牋for?i=0;爄=n-1;爄++);爎eturn牋牋牋牋for?j=0;爅=m-1;爅++);爎eturn牋牋牋牋牋or?j={爅j=i*m+j;爂[jj]=0.0;+)牋牋牋牋牋牋for?kk=0;爇k=m-1;爇k++)rn(js);}牋牋牋牋牋牋牋g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk];牋牋牋牋牋}g[jj]牋牋牋牋for?i=0;爄=n-1;爄++)+kk]*e[牋牋牋牋牋or?i={爅j=(ii-1)*n+i;爔[jj]=0.0;k]牋牋牋牋牋牋for?j=0;爅=n-1;爅++)k]*e[j*m+牋牋牋牋牋牋牋x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j];牋牋牋牋牋}x[jj]牋牋牋牋for?i=0;爄=m-1;爄++)+j]*x[(牋牋牋牋牋or?i={爅j=i*l;燽[jj]=y[(ii-1)*m+i];[(牋牋牋牋牋牋for?j=0;爅=n-1;爅++)];[(ii-2牋牋牋牋牋牋牋b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j];牋牋牋牋牋}b[jj]牋牋牋牋for?i=0;爄=n-1;爄++)+j]*x[(牋牋牋牋牋or?i={爅j=(ii-1)*n+i;-牋牋牋牋牋牋for?j=0;爅=m-1;爅++)]*x[(ii-1牋牋牋牋牋牋牋x[jj]=x[jj]+g[i*m+j]*b[j*l];*n+j];牋牋牋牋牋}x[jj]牋牋牋牋if?iik)x[jj]牋牋牋牋牋f?ii{爁or?i=0;爄=n-1;爄++)i*m+牋牋牋牋牋牋for?j=0;爅=n-1;爅++)]*b[j*l];牋牋牋牋牋牋牋or?j=0;{爅j=i*l+j;燼[jj]=0.0;爅+牋牋牋牋牋牋牋牋for?kk=0;爇k=m-1;爇k++)*l];