易拉罐的博客

心静自然凉

日志

关于我

易拉罐bb

aaa

文章分类

转 a class for kalman filter 卡尔曼滤波的例子

2010-03-25 14:15:50| 分类：机器视觉 | 标签： |举报 |字号大中小订阅

下载LOFTER 我的照片书 |

因为在研究中使用了kalman 滤波，这是一个挺难理解的控制理论，我花了好长时间才了解一些基本的概念，opencv虽然提供了一个实例，但是这个例子基于c的，不容易看懂，也不好重用，后来我整理了一个简单的类来，期间在论坛上有一个handsome & romantic 的法国小伙也在研究这个滤波，后来我给了他程序，他修改后发给了我，所以这里的代码也有他的部分，算是中法合作产品 :)

对于kalman的初学者来讲，像我这样没什么数学功底的人，看教科书真是很累，说实在的，我觉得老外的基础理论的书都很评议近人，不像国内那些教授搞得那么悬虚，

初学者可以参考

http://bbs.matwav.com/index.jsp 研学论坛有几篇通俗易懂的中文解释

http://www.cs.unc.edu/~welch/kalman/ 这里是老外综合的kalman基地，很不错的。

代码示例:

============================kalman.h================================

// kalman.h: interface for the kalman class.
//
//////////////////////////////////////////////////////////////////////

#if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)
#define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_

#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

#include <math.h>
#include "cv.h"

class kalman
{
public:
void init_kalman(int x,int xv,int y,int yv);
CvKalman* cvkalman;
CvMat* state;
CvMat* process_noise;
CvMat* measurement;
const CvMat* prediction;
CvPoint2D32f get_predict(float x, float y);
kalman(int x=0,int xv=0,int y=0,int yv=0);
//virtual ~kalman();

};

#endif // !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)

============================kalman.cpp================================

#include "kalman.h"
#include <stdio.h>

/* tester de printer toutes les valeurs des vecteurs...*/
/* tester de changer les matrices du noises */
/* replace state by cvkalman->state_post ??? */

CvRandState rng;
const double T = 0.1;
kalman::kalman(int x,int xv,int y,int yv)
{
    cvkalman = cvCreateKalman( 4, 4, 0 );
    state = cvCreateMat( 4, 1, CV_32FC1 );
    process_noise = cvCreateMat( 4, 1, CV_32FC1 );
    measurement = cvCreateMat( 4, 1, CV_32FC1 );
    int code = -1;

    /* create matrix data */
     const float A[] = {
   1, T, 0, 0,
   0, 1, 0, 0,
   0, 0, 1, T,
   0, 0, 0, 1
  };

     const float H[] = {
    1, 0, 0, 0,
    0, 0, 0, 0,
   0, 0, 1, 0,
   0, 0, 0, 0
  };

     const float P[] = {
    pow(320,2), pow(320,2)/T, 0, 0,
   pow(320,2)/T, pow(320,2)/pow(T,2), 0, 0,
   0, 0, pow(240,2), pow(240,2)/T,
   0, 0, pow(240,2)/T, pow(240,2)/pow(T,2)
    };

     const float Q[] = {
   pow(T,3)/3, pow(T,2)/2, 0, 0,
   pow(T,2)/2, T, 0, 0,
   0, 0, pow(T,3)/3, pow(T,2)/2,
   0, 0, pow(T,2)/2, T
   };

     const float R[] = {
   1, 0, 0, 0,
   0, 0, 0, 0,
   0, 0, 1, 0,
   0, 0, 0, 0
   };


    cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );

    cvZero( measurement );

    cvRandSetRange( &rng, 0, 0.1, 0 );
    rng.disttype = CV_RAND_NORMAL;

cvRand( &rng, state );

    memcpy( cvkalman->transition_matrix->data.fl, A, sizeof(A));
    memcpy( cvkalman->measurement_matrix->data.fl, H, sizeof(H));
    memcpy( cvkalman->process_noise_cov->data.fl, Q, sizeof(Q));
    memcpy( cvkalman->error_cov_post->data.fl, P, sizeof(P));
    memcpy( cvkalman->measurement_noise_cov->data.fl, R, sizeof(R));
    //cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5) );
    //cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1));
//cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) );

/* choose initial state */

    state->data.fl[0]=x;
    state->data.fl[1]=xv;
    state->data.fl[2]=y;
    state->data.fl[3]=yv;
    cvkalman->state_post->data.fl[0]=x;
    cvkalman->state_post->data.fl[1]=xv;
    cvkalman->state_post->data.fl[2]=y;
    cvkalman->state_post->data.fl[3]=yv;

cvRandSetRange( &rng, 0, sqrt(cvkalman->process_noise_cov->data.fl[0]), 0 );
cvRand( &rng, process_noise );

}

CvPoint2D32f kalman::get_predict(float x, float y){

    /* update state with current position */
    state->data.fl[0]=x;
    state->data.fl[2]=y;

    /* predict point position */
    /* x'k=A鈥k+B鈥k
       P'k=A鈥k-1*AT + Q */
    cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl[0]), 0 );
    cvRand( &rng, measurement );

     /* xk=A?xk-1+B?uk+wk */
    cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post );

    /* zk=H?xk+vk */
    cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement );

    /* adjust Kalman filter state */
    /* Kk=P'k鈥T鈥?H鈥'k鈥T+R)-1
       xk=x'k+Kk鈥?zk-H鈥'k)
       Pk=(I-Kk鈥)鈥'k */
    cvKalmanCorrect( cvkalman, measurement );
    float measured_value_x = measurement->data.fl[0];
    float measured_value_y = measurement->data.fl[2];

const CvMat* prediction = cvKalmanPredict( cvkalman, 0 );
    float predict_value_x = prediction->data.fl[0];
    float predict_value_y = prediction->data.fl[2];

return(cvPoint2D32f(predict_value_x,predict_value_y));
}

void kalman::init_kalman(int x,int xv,int y,int yv)
{
state->data.fl[0]=x;
    state->data.fl[1]=xv;
    state->data.fl[2]=y;
    state->data.fl[3]=yv;
    cvkalman->state_post->data.fl[0]=x;
    cvkalman->state_post->data.fl[1]=xv;
    cvkalman->state_post->data.fl[2]=y;
    cvkalman->state_post->data.fl[3]=yv;
}

作者Blog：http://blog.csdn.net/hardVB/

评论这张

转发至微博

阅读(777)| 评论(0)

历史上的今天

this.p={  m:2,
              b:2,
              loftPermalink:'',
              id:'fks_080065082081084067087084095095085085082071082086082074092',
              blogTitle:'转 a class for kalman filter 卡尔曼滤波的例子',
              blogAbstract:'<SPAN\>    <P\>因为在研究中使用了kalman 滤波，这是一个挺难理解的控制理论，我花了好长时间才了解一些基本的概念，opencv虽然提供了一个实例，但是这个例子基于c的，不容易看懂，也不好重用，后来我整理了一个简单的类来，期间在论坛上有一个handsome & romantic 的法国小伙也在研究这个滤波，后来我给了他程序，他修改后发给了我，所以这里的代码也有他的部分，算是中法合作产品 :)</P\>  <P\>对于kalman的初学者来讲，像我这样没什么数学功底的人，看教科书真是很累，说实在的，我觉得老外的基础理论的书都很评议近人，不像国内那些教授搞得那么悬虚，</P\>  <P\>初学者可以参考</P\>  <P\><A rel=\"nofollow\" href=\"http://bbs.matwav.com/index.jsp\"  \><FONT color=\"#a40000\" \>http://bbs.matwav.com/index.jsp</FONT\></A\></P\></SPAN\>',
              blogTag:'',
              blogUrl:'blog/static/133460498201022521550194',
              isPublished:1,
              istop:false,
              type:2,
              modifyTime:1317336325807,
              publishTime:1269497750194,
              permalink:'blog/static/133460498201022521550194',
              commentCount:0,
              mainCommentCount:0,
              recommendCount:0,
              bsrk:-100,
              publisherId:0,
              recomBlogHome:false,
              currentRecomBlog:false,
              attachmentsFileIds:[],
              vote:{},
              groupInfo:{},
              friendstatus:'none',
              followstatus:'unFollow',
              pubSucc:'',
              visitorProvince:'',
              visitorCity:'',
              visitorNewUser:false,
              postAddInfo:{},
              mset:'000',
              mcon:'',
              srk:-100,
              remindgoodnightblog:false,
              isBlackVisitor:false,
              isShowYodaoAd:false,
              hostIntro:'aaa',
              hmcon:'1',
              selfRecomBlogCount:'0',
              lofter_single:'<iframe width="140" height="560" style="overflow:hidden;" src="http://www.lofter.com/mailEntry.do?blogad=1&blog" frameBorder="0"></iframe>'
            }

{list a as x}
    {if !!x}
    <div class="iblock nbw-fce nbw-f40">
      <a class="fc03 noul" target="_blank" hidefocus="true" href="http://blog.163.com/${x.visitorName}/">
      {if x.visitorName==visitor.userName}
      <img alt="${x.visitorNickname|escape}" onerror="this.src=location.f40" class="cwd bdwa bdc0" src="${fn1(x.visitorName)}&r=${visitor.imageUpdateTime}"/>
      {else}
      <img alt="${x.visitorNickname|escape}" onerror="this.src=location.f40" class="cwd bdwa bdc0" src="${fn1(x.visitorName)}"/>
      {/if}
      </a>
      <div class="cwd vname thide">
        {if x.moveFrom=='wap'}
          <a class="noul pnt" target="_blank" href="http://blog.163.com/services/wapblog.html?frompersonalbloghome"><span title="来自网易手机博客" class="iblock wapIcon"> </span></a>
        {elseif x.moveFrom=='iphone'}
          <a class="noul pnt" target="_blank"><span title="来自iPhone客户端" class="iblock iphoneIcon"> </span></a>
        {elseif x.moveFrom=='android'}
          <a class="noul pnt" target="_blank"><span title="来自Android客户端" class="iblock androidIcon"> </span></a>
        {elseif x.moveFrom=='mobile'}
          <a class="noul pnt" target="_blank" href="http://blog.163.com/services/emsblog.html?frompersonalbloghome"><span title="来自网易短信写博" class="iblock wapIcon"> </span></a>
        {/if}
        <a class="fc03 m2a"  target="_blank" hidefocus="true" href="http://blog.163.com/${x.visitorName}/">
          ${fn(x.visitorNickname,8)|escape}
        </a>
      </div>
    </div>
    {/if}
    {/list}

<#--最新日志，群博日志--> <#--推荐日志-->

<p class="fc06">推荐过这篇日志的人：</p>
    <div>
      {list a as x}
      {if !!x}
      <div class="iblock nbw-fce nbw-f40">
        <a class="fc03 noul" target="_blank" hidefocus="true" href="http://blog.163.com/${x.recommenderName}/">
        <img alt="${x.recommenderNickname|escape}" onerror="this.src=location.f40" class="cwd bdwa bdc0" src="${fn1(x.recommenderName)}"/>
        </a>
        <div class="cwd thide">
          <a class="fc03 m2a" target="_blank" hidefocus="true" href="http://blog.163.com/${x.recommenderName}/">
            ${fn(x.recommenderNickname,6)|escape}
          </a>
        </div>
      </div>
      {/if}
      {/list}
    </div>
    {if !!b&&b.length>0}
    <p  class="fc06">他们还推荐了：</p>
    <ul>
    {list b as y}
      {if !!y}
        <li class="rrb"><span class="iblock">·</span><a class="fc03 m2a" target="_blank" href="http://blog.163.com/${y.recommendBlogPermalink}/?from=blog/static/133460498201022521550194">${y.recommendBlogTitle|escape}</a></li>
      {/if}
    {/list}
    </ul>
    {/if}

<#--引用记录--> <#--博主推荐--> <#--随机阅读--> <#--首页推荐--> <#--历史上的今天--> <#--被推荐日志--> <#--上一篇，下一篇--> <#-- 热度 -->

{list a as x}
    {if !!x}
    <div class="hotItem iblock nbw-fce nbw-f40">
      <a class="fc03 noul" target="_blank" hidefocus="true" href="http://blog.163.com/${x.publisherUsername}/">
      {if x.publisherUsername==visitor.userName}
      <img alt="${x.publisherNickname|escape}" onerror="this.src=location.f40" class="cwd bdwa bdc0" src="${fn1(x.publisherUsername)}&r=${visitor.imageUpdateTime}"/>
      {else}
      <img alt="${x.publisherNickname|escape}" onerror="this.src=location.f40" class="cwd bdwa bdc0" src="${fn1(x.publisherUsername)}"/>
      {/if}
      </a>
      <div class="cwd vname thide">
        <a class="fc03 m2a"  target="_blank" hidefocus="true" href="http://blog.163.com/${x.publisherUsername}/">
          ${fn(x.publisherNickname,8)|escape}
        </a>
      </div>
      <a class="f-myLikeIcons hottype {if x.type==1} js-liketype{elseif x.type==2} js-reblogtype{elseif x.type==3} js-sharetype{else}{/if}" target="_blank" hidefocus="true" href="http://blog.163.com/${x.publisherUsername}/"> </a>
    </div>
    {/if}
    {/list}

<#-- 网易新闻广告 --> <#--右边模块结构--> <#--评论模块结构--> <#--引用模块结构--> <#--博主发起的投票-->

页脚

我的照片书 - 手机博客 - 下载LOFTER APP - 订阅此博客

易拉罐的博客

导航

日志

转 a class for kalman filter 卡尔曼滤波的例子

历史上的今天

最近读者

热度

评论

页脚