kalman.c

用C语言实现的kalman滤波程序（一般网上只有用MATLAB实现的）。

#include "stdio.h"
#include "stdlib.h"
#include "math.h"
int brinv(double a[],int n)
  { 
  	int *is,*js,i,j,k,l,u,v;
    double d,p;
    is=malloc(n*sizeof(int));
    js=malloc(n*sizeof(int));
    for (k=0; k<=n-1; k++)
      { d=0.0;
        for (i=k; i<=n-1; i++)
        for (j=k; j<=n-1; j++)
          { l=i*n+j; p=fabs(a[l]);
            if (p>d) { d=p; is[k]=i; js[k]=j;}
          }
        if (d+1.0==1.0)
          { free(is); free(js); printf("err**not inv\n");
            return(0);
          }
        if (is[k]!=k)
          for (j=0; j<=n-1; j++)
            { u=k*n+j; v=is[k]*n+j;
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        if (js[k]!=k)
          for (i=0; i<=n-1; i++)
            { u=i*n+k; v=i*n+js[k];
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        l=k*n+k;
        a[l]=1.0/a[l];
        for (j=0; j<=n-1; j++)
          if (j!=k)
            { u=k*n+j; a[u]=a[u]*a[l];}
        for (i=0; i<=n-1; i++)
          if (i!=k)
            for (j=0; j<=n-1; j++)
              if (j!=k)
                { u=i*n+j;
                  a[u]=a[u]-a[i*n+k]*a[k*n+j];
                }
        for (i=0; i<=n-1; i++)
          if (i!=k)
            { u=i*n+k; a[u]=-a[u]*a[l];}
      }
    for (k=n-1; k>=0; k--)
      { if (js[k]!=k)
          for (j=0; j<=n-1; j++)
            { u=k*n+j; v=js[k]*n+j;
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
        if (is[k]!=k)
          for (i=0; i<=n-1; i++)
            { u=i*n+k; v=i*n+is[k];
              p=a[u]; a[u]=a[v]; a[v]=p;
            }
      }
    free(is); free(js);
    return(1);
  }
  /******************************************************************/
  
 
  int klman(int n,int m,int k,double f[],double q[],double r[],double h[],double y[],double x[],double p[],double 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<n) 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=brinv(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 (ii<k)
          { 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; p[jj]=q[jj];
                for (kk=0; kk<=n-1; kk++)
                  p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk];
              }
          }
      }
    free(e); free(a); free(b);
    return(js);
  }

/***********************************************************************/


  main()
  { int i,j,js;
  	FILE *fp;
    void keklm5();
    double p[3][3],x[200][3],y[200][1],g[3][1],t,s;
    static double f[3][3]={{1.0,0.05,0.00125},
                 {0.0,1.0,0.05},{0.0,0.0,1.0}};
    static double q[3][3]={{0.25,0.0,0.0},
                 {0.0,0.25,0.0},{0.0,0.0,0.25}};
    static double r[1][1]={0.25};
    static double h[1][3]={1.0,0.0,0.0};
   if(( fp=fopen("out.txt","w+"))==NULL)

    	 printf("Open file error!\n");
    for (i=0; i<=2; i++)
    for (j=0; j<=2; j++) p[i][j]=0.0;
    for (i=0; i<=199; i++)
    for (j=0; j<=2; j++) x[i][j]=0.0;
    keklm5(200,y);
    for (i=0; i<=199; i++)
      { t=0.05*i;
        y[i][0]=5.0-2.0*t+3.0*t*t+y[i][0];
      }
    js=klman(3,1,200,f,q,r,h,y,x,p,g);
    if (js!=0)
      { printf("\n");
        printf("   t       s            y            ");
        printf("x(0)         x[1]         x(2)   \n");
        for (i=0; i<=199; i=i+5)
          { t=0.05*i; s=5.0-2.0*t+3.0*t*t;
            printf("%6.2f  %e  %e  %e  %e  %e\n",
                   t,s,y[i][0],x[i][0],x[i][1],x[i][2]);
            fprintf(fp,"%f\t%f\t%f\n",x[i][0],x[i][1],x[i][2]);
          }
        printf("\n");
      }
  }

  static void keklm5(n,y)
  int n;
  double y[];
  { int i,j,m;
    double s,w,v,r,t;
    s=65536.0; w=2053.0; v=13849.0; r=0.0;
    for (i=0; i<=n-1; i++)
      { t=0.0;
        for (j=0; j<=11; j++)
          { r=w*r+v; m=r/s; r=r-m*s; t=t+r/s;}
        y[i]=0.5*(t-6.0);
      }
    return;
  }