卡尔曼滤波器算法实现C语言实现概要.pdf

/***************************************** ****** 卡尔曼滤波器算法实

现 ********* ****** c 语言实现 *********** ****** ****** ******实现函数：int

klman(n,m,k,f,q,r,h,y,x,p,g ****** ******详细意义见算法说明 ****** ******如果要

编制 matlab 实现程序，可以参考 kalman_matlab.m ******

**************************************/ #include "stdlib.h" int

klman(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; extern int brinv(; e=malloc(m*m*sizeof(double; l=m; if (l

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++

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 (ii { 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; kk++ a[jj]=a[jj]-

g[i*m+kk]*h[kk*n+j]; if (i==j a[jj]=1.0+a[jj]; } for (i=0; i<=n-1; i++ for (j=0; j<=n-1;

j++ { jj=i*l+j; b[jj]=0.0; for (kk=0; kk<=n-1; kk++ b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j]; } 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<=n-1; kk++

a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk]; } for (i=0; i<=n-1; i++ for (j=0; j<=n-1; j++ { jj=i*n+j;