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

下载文档 降价啦

3
0
约7.51千字
约 10页
2017-01-13 发布于重庆
举报
版权申诉
保障服务

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

1、本文档共10页，可阅读全部内容。
2、有哪些信誉好的足球投注网站（book118）网站文档一经付费（服务费），不意味着购买了该文档的版权，仅供个人/单位学习、研究之用，不得用于商业用途，未经授权，严禁复制、发行、汇编、翻译或者网络传播等，侵权必究。
3、本站所有内容均由合作方或网友上传，本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺！文档内容仅供研究参考，付费前请自行鉴别。如您付费，意味着您自己接受本站规则且自行承担风险，本站不退款、不进行额外附加服务；查看《如何避免下载的几个坑》。如果您已付费下载过本站文档，您可以点击这里二次下载。
4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等，请点击“版权申诉”（推荐），也可以打举报电话：400-050-0827(电话支持时间：9:00-18:30)。

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

卡尔曼滤波算法实现代码其c语言实现代码如下： #include?stdlib.h??#include?rinv.c??int?lman(n,m,k,f,q,r,h,y,x,p,g)??int?n,m,k;??double?f[],q[],r[],h[],y[],x[],p[],g[];??{?int?i,j,kk,ii,l,jj,js;????double?*e,*a,*b;????e=malloc(m*m*sizeof(double));????l=m;????if?(ln)?l=n;????a=malloc(l*l*sizeof(double));????b=malloc(l*l*sizeof(double));????for?(i=0;?i=n-1;?i++)??????for?(j=0;?j=n-1;?j++)????????{?ii=i*l+j;?a[ii]=0.0;??????????for?(kk=0;?kk=n-1;?kk++)????????????a[ii]=a[ii]+p[i*n+kk]*f[j*n+kk];????????}????for?(i=0;?i=n-1;?i++)??????for?(j=0;?j=n-1;?j++)????????{?ii=i*n+j;?p[ii]=q[ii];??????????for?(kk=0;?kk=n-1;?kk++)????????????p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j];????????}????for?(ii=2;?ii=k;?ii++)??????{?for?(i=0;?i=n-1;?i++)????????for?(j=0;?j=m-1;?j++)??????????{?jj=i*l+j;?a[jj]=0.0;????????????for?(kk=0;?kk=n-1;?kk++)??????????????a[jj]=a[jj]+p[i*n+kk]*h[j*n+kk];??????????}????????for?(i=0;?i=m-1;?i++)????????for?(j=0;?j=m-1;?j++)??????????{?jj=i*m+j;?e[jj]=r[jj];????????????for?(kk=0;?kk=n-1;?kk++)??????????????e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];??????????}????????js=rinv(e,m);????????if?(js==0)???????????{?free(e);?free(a);?free(b);?return(js);}????????for?(i=0;?i=n-1;?i++)????????for?(j=0;?j=m-1;?j++)??????????{?jj=i*m+j;?g[jj]=0.0;????????????for?(kk=0;?kk=m-1;?kk++)??????????????g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk];??????????}????????for?(i=0;?i=n-1;?i++)??????????{?jj=(ii-1)*n+i;?x[jj]=0.0;????????????for?(j=0;?j=n-1;?j++)??????????????x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j];??????????}????????for?(i=0;?i=m-1;?i++)??????????{?jj=i*l;?b[jj]=y[(ii-1)*m+i];????????????for?(j=0;?j=n-1;?j++)??????????????b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j];??????????}????????for?(i=0;?i=n-1;?i++)??????????{?jj=(ii-1)*n+i;????????????for?(j=0;?j=m-1;?j++)??????????????x[jj]=x[jj]+g[i*m+j]*b[j*l];??????????}????????if?(iik)??????????{?for?(i=0;?i=n-1;?i++)????????????for?(j=0;?j=n-1;?j++)??????????????{?jj=i*l+j;?a[jj]=0.0;????????????????for?(kk=0;?kk=m-1;