久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步,快速開始

搜索
查看: 5000|回復: 2
打印 上一主題 下一主題
收起左側

Kalman 卡爾曼濾波算法

[復制鏈接]
跳轉到指定樓層
樓主
ID:80436 發表于 2015-5-21 22:46 | 只看該作者 回帖獎勵 |倒序瀏覽 |閱讀模式
#include "stdlib.h"
  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 (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=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 (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);
  }


評分

參與人數 1黑幣 +5 收起 理由
haier + 5 很給力!

查看全部評分

分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏1 分享淘帖 頂 踩
回復

使用道具 舉報

沙發
ID:38150 發表于 2017-5-21 23:00 | 只看該作者
看不懂
回復

使用道具 舉報

板凳
ID:211258 發表于 2017-6-15 16:31 | 只看該作者
非常好
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板: 偷拍自拍网址 | 日韩精品| 欧美一级高清片 | 久久久男人的天堂 | 国产精品污污视频 | 中文字幕影院 | 不卡视频一区二区三区 | 日本精品在线一区 | 国产日韩欧美精品 | 日韩高清中文字幕 | 天天天天操 | 国产在线h | 亚洲成人精选 | 国产成人一区二区三区久久久 | 久久久久国产一区二区三区四区 | 亚洲成av人影片在线观看 | 成人激情视频免费在线观看 | 欧美日在线 | 国产精品久久久久久久久久 | 欧美综合视频在线 | 欧美激情国产精品 | 亚洲欧美国产精品一区二区 | 天天操天天射天天 | 欧美午夜久久 | 国产免费观看久久黄av片涩av | 五月香婷婷 | 欧美一区二区免费在线 | 国产乱码高清区二区三区在线 | 91影视| 亚洲精品国产综合区久久久久久久 | 欧美1级 | 精品成人av| 国产激情一区二区三区 | 欧美亚洲视频 | 夜夜操操操 | 国产69精品久久99不卡免费版 | 久久夜色精品国产 | 一区二区三区高清不卡 | 久久久久久久成人 | 国产精品久久九九 | 亚洲成人免费 |