滤波-卡尔曼滤波-互补滤波

滤波方法及其原理， 卡尔曼滤波，互补滤波

  #include “stdlib.h“
  #include “math.h“
  #include “stdio.h“
  int brinv（an）
  int n;
  double *a;
  { int *is*jsijkluv;
    double dp;
    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）;
  }
  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2008  2011-07-30 19:05  Simple Kalman Filter in C.txt

----------- ---------  ---------- -----  ----

                 2008                    1