卡尔曼滤波算法(c--c++两种实现代码).docxVIP

第二部分施工组织设计 第二部分施工组织设计 页脚内容 页脚内容 PAGE # 卡尔曼滤波算法实现代码 C++实现代码如下： ============================kalma n.h================================ // kalma n.h: in terface for the kalma n class. // ///////////////////////////////////////////////////////////////////// / #if !defi ned(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED _ #defi ne AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_ #if _MSC_VER 1000 #pragma once #e ndif // _MSC_VER 1000 #in clude math.h #i nclude cv.h class kalma n { public : void init_kalman( int x, int xv, int y, int yv); CvKalma n* cvkalma n; CvMat* state; CvMat* process_ no ise; CvMat* measureme nt; con st CvMat* predictio n; CvPoi nt2D32f get_predict( float x, float y); kalma n( int x=0, int xv=0. int y=0, int yv=0); //virtual ~kalma n(); }; #en dif // !defi ned(AFX_KALMAN_H ED3D740F 01D2 4616 8B74 8BF57636F2C0 ED_) kalma n.cpp #in clude kalma n.h #in clude stdio.h /* tester de pr in ter toutes les valeurs des vecteurs - */ /* tester de cha nger les matrices du no ises */ /* replace state by cvkalma n-state_post ??? */ CvRa ndState rng; const double T = 0.1; kalma n::kalma n( int x, int xv, int y, int yv) { TOC \o 1-5 \h \z cvkalma n = cvCreateKalma n( 4, 4, 0 ); state = cvCreateMat( 4, 1, CV_32FC1 ); process, noise = cvCreateMat( 4, 1, CV_32FC1 ); measureme nt = cvCreateMat( 4, 1, CV_32FC1 ); int code = -1; /* create matrix data */ con st float A[] = { 1, T, 0, 0, 0, 1, 0, 0, 0, 0, 1, T, 0, 0, 0, 1 }； con st float H[] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }； con st 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) }； con st 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 }； con st float R[] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }; cvRa ndl nit( rng, 0, 1, -1, CV_RAND_UNI ); cvZero( measureme nt ); cvRa ndSetRa nge( rng, 0,