kalman.c

一个标准卡尔曼滤波程序

  #include <stdio.h>
  #include <stdlib.h>
  #include <math.h>
  
  static void eklm5(int n,double y[]);
  int rinv(double a[],int n);
  int lman(int n,int m,int k,double f[],double q[],double r[],double h[],double y[],double x[],double p[],double g[]);
  
  void main()
  { int i,j,js;
    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};
    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;
    eklm5(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=lman(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]);
          }
        printf("\n");
      }
  }

  static void eklm5(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=(int)(r/s); r=r-m*s; t=t+r/s;}
        y[i]=0.5*(t-6.0);
      }
    return;
  }
  
  int rinv(double a[],int n)
  { int *is,*js,i,j,k,l,u,v;
    double d,p;
    is=(int *)malloc(n*sizeof(int));
    js=(int *)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 lman(int n,int m,int k,double f[],double q[],double r[],double h[],double y[],double x[],double p[],double g[])
  //n为动态系统的维数
  //m为观测系统的维数
  //k为观测序列的长度
  //f[n][n]为系统状态转移矩阵
  //q[n][n]为模型噪声wk的协方差阵
  //r[m][m]为观测噪声vk的协方差阵
  //h[m][n]为观测矩阵
  //y[k][m]观测向量序列，其中y(i,j)表示第i时刻的观测向量的第j个分量
  //x[k][n]其中x(0,j)存放给定的初始值，其余各行返回状态向量估序列，x(i,j)表示第i时刻的状态向量估值的第j个分量
  //p[n][n]存放初值p0,返回最后时刻的估计误差协方差阵
  //g[n][m]返回最后时刻的稳定增益矩阵
  { int i,j,kk,ii,l,jj,js;
    double *e,*a,*b;
    e=(double *)malloc(m*m*sizeof(double));
    l=m;
    if (l<n) l=n;
    a=(double *)malloc(l*l*sizeof(double));
    b=(double *)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 (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 (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];//Pk*H(T)k
          }
        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];//Hk*Pk*H(T)k+Rk
          }
        js=rinv(e,m);//Hk*Pk*H(T)k+Rk求逆
        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];//求得Gk
          }
        /*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);
  }