CvKalman
Kalman 濾波器狀態
typedef struct CvKalman
{
int MP; /* 測量向量維數 */
int DP; /* 狀態向量維數 */
int CP; /* 控制向量維數 */
/* 向後相容欄位 */
#if 1
float* PosterState; /* =state_pre->data.fl */
float* PriorState; /* =state_post->data.fl */
float* DynamMatr; /* =transition_matrix->data.fl */
float* MeasurementMatr; /* =measurement_matrix->data.fl */
float* MNCovariance; /* =measurement_noise_cov->data.fl */
float* PNCovariance; /* =process_noise_cov->data.fl */
float* KalmGainMatr; /* =gain->data.fl */
float* PriorErrorCovariance;/*
=error_cov_pre->data.fl */
float* PosterErrorCovariance;/*
=error_cov_post->data.fl */
float* Temp1; /* temp1->data.fl */
float* Temp2; /* temp2->data.fl */
#endif
CvMat* state_pre; /* 預測狀態 (x'(k)):
x(k)=A*x(k-1)+B*u(k) */
CvMat* state_post; /* 矯正狀態 (x(k)):
x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
CvMat* transition_matrix; /* 狀態傳遞矩陣 state transition matrix (A) */
CvMat* control_matrix; /* 控制矩陣 control matrix (B)
(如果沒有控制，則不使用它)*/
CvMat* measurement_matrix; /* 測量矩陣 measurement matrix (H) */
CvMat* process_noise_cov; /* 過程雜訊協方差矩陣
process noise covariance matrix (Q) */
CvMat* measurement_noise_cov; /*
測量雜訊協方差矩陣
measurement noise covariance matrix (R) */
CvMat*
error_cov_pre; /* 先驗誤差計協方差矩陣
priori
error estimate covariance matrix (P'(k)):
P'(k)=A*P(k-1)*At +
Q)*/
CvMat*
gain; /* Kalman 增益矩陣 gain matrix (K(k)):
K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
CvMat*
error_cov_post; /* 後驗錯誤估計協方差矩陣
posteriori
error estimate covariance matrix (P(k)):
P(k)=(I-K(k)*H)*P'(k) */
CvMat*
temp1; /* 臨時矩陣 temporary matrices */
CvMat*
temp2;
CvMat*
temp3;
CvMat*
temp4;
CvMat*
temp5;
}
CvKalman;
結構 CvKalman 用來保存 Kalman 濾波器狀態。它由函數 cvCreateKalman 創建，由函數f
cvKalmanPredict 和
cvKalmanCorrect 更新，由
cvReleaseKalman 釋放. 通常該結構是為標準 Kalman 所使用的 (符號和公式都借自非常優秀的 Kalman 教程 [Welch95]):
系統運動方程：
系統觀測方程：
其中:
xk(xk −
1) - 系統在時刻 k
(k-1) 的狀態向量 （state of the system at the moment k (k-1)）
zk -
在時刻 k 的系統狀態測量向量 （measurement of the system state at the moment k）
uk -
應用於時刻 k 的外部控制 (external control applied at the moment k)
wk 和 vk 分別為正態分佈的運動和測量雜訊
p(w) ~ N(0,Q)
p(v) ~ N(0,R),
即,
Q - 運動雜訊的相關矩陣，常量或變數
R - 測量雜訊的相關矩陣，常量或變數
對標準 Kalman 濾波器，所有矩陣: A, B, H, Q 和 R 都是通過 cvCreateKalman 在分配結構 CvKalman 時初始化一次。但是，同樣的結構和函數，通過在當前系統狀態鄰域中線性化擴展 Kalman 濾波器方程，可以用來模擬擴展 Kalman 濾波器，在這種情況下， A, B, H (也許還有 Q 和 R) 在每一步中都被更新。
CreateKalman
分配 Kalman 濾波器結構
CvKalman* cvCreateKalman( int dynam_params, int measure_params, int
control_params=0 );
dynam_params
狀態向量維數
measure_params
測量向量維數
control_params
控制向量維數
函數 cvCreateKalman 分配
CvKalman 以及它的所有矩陣和初始參數
ReleaseKalman
釋放 Kalman 濾波器結構
void cvReleaseKalman( CvKalman** kalman );
kalman
指向 Kalman 濾波器結構的雙指標
函數 cvReleaseKalman 釋放結構
CvKalman 和裏面所有矩陣
KalmanPredict
估計後來的模型狀態
const CvMat* cvKalmanPredict( CvKalman* kalman, const CvMat* control=NULL
);
#define cvKalmanUpdateByTime cvKalmanPredict
kalman
Kalman 濾波器狀態
control
控制向量 (uk), 如果沒有外部控制 (control_params=0) 應該為
NULL
函數 cvKalmanPredict 根據當前狀態估計後來的隨機模型狀態，並存儲於 kalman->state_pre:
,
其中
x'k 是預測狀態 (kalman->state_pre),
xk −
1 是前一步的矯正狀態 (kalman->state_post)，應該在開始的某個地方初始化，即缺省為零向量，
uk 是外部控制(control 參數),
P'k 是先驗誤差相關矩陣 (kalman->error_cov_pre)
Pk −
1 是前一步的後驗誤差相關矩陣(kalman->error_cov_post),應該在開始的某個地方初始化，即缺省為單位矩陣.
函數返回估計得到的狀態值
KalmanCorrect
調節模型狀態
const CvMat* cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement
);
#define cvKalmanUpdateByMeasurement cvKalmanCorrect
kalman
被更新的 Kalman 結構的指標
measurement
指向測量向量的指標，向量形式為 CvMat
函數 cvKalmanCorrect 在給定的模型狀態的測量基礎上，調節隨機模型狀態：
其中
zk -
給定測量(mesurement parameter)
Kk -
Kalman "增益" 矩陣
函數存儲調節狀態到 kalman->state_post 中並且輸出時返回它。
[Example]
下面實現了一個簡單的跟蹤小程式，直接給出程式源碼：
void CSLAMApplicationView::OnEKFTracking()
{
// Initialize Kalman filter object, window, number generator, etc
cvNamedWindow( "Kalman", 1 );//創建視窗，當為的時候，表示視窗大小自動設定
CvRandState rng;
cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );/* CV_RAND_UNI 指定為均勻分佈類型、亂數種子為-1 */
IplImage* img = cvCreateImage( cvSize(500,500), 8, 3 );
CvKalman* kalman = cvCreateKalman( 2, 1, 0 );/*狀態向量為維，觀測向量為維，無激勵輸入維*/
// State is phi, delta_phi - angle and angular velocity
// Initialize with random guess
CvMat* x_k = cvCreateMat( 2, 1, CV_32FC1 );/*創建行列、元素類型為CV_32FC1，元素為位元單通道浮點類型矩陣。*/
cvRandSetRange( &rng, 0, 0.1, 0 );/*設置亂數範圍，亂數服從正態分佈，均值為，均方差為.1，通道個數為*/
rng.disttype = CV_RAND_NORMAL;
cvRand( &rng, x_k ); /*隨機填充陣列*/
// Process noise
CvMat* w_k = cvCreateMat( 2, 1, CV_32FC1 );
// Measurements, only one parameter for angle
CvMat* z_k = cvCreateMat( 1, 1, CV_32FC1 );/*定義觀測變數*/
cvZero( z_k ); /*矩陣置零*/
// Transition matrix F describes model parameters at and k and k+1
const float F[] = { 1, 1, 0, 1 }; /*狀態轉移矩陣*/
memcpy( kalman->transition_matrix->data.fl, F, sizeof(F));
/*初始化轉移矩陣，行列，具體見CvKalman* kalman = cvCreateKalman( 2, 1, 0 );*/
// Initialize other Kalman parameters
cvSetIdentity( kalman->measurement_matrix, cvRealScalar(1) );/*觀測矩陣*/
cvSetIdentity( kalman->process_noise_cov, cvRealScalar(1e-5) );/*過程雜訊*/
cvSetIdentity( kalman->measurement_noise_cov, cvRealScalar(1e-1) );/*觀測雜訊*/
cvSetIdentity( kalman->error_cov_post, cvRealScalar(1) );/*後驗誤差協方差*/
// Choose random initial state
cvRand( &rng, kalman->state_post );/*初始化狀態向量*/
// Make colors
CvScalar yellow = CV_RGB(255,255,0);/*依次為紅綠藍三色*/
CvScalar white = CV_RGB(255,255,255);
CvScalar red = CV_RGB(255,0,0);
while( 1 ){
// Predict point position
const CvMat* y_k = cvKalmanPredict( kalman, 0 );/*激勵項輸入為*/
// Generate Measurement (z_k)
cvRandSetRange( &rng, 0, sqrt( kalman->measurement_noise_cov->data.fl[0] ), 0 );/*設置觀測雜訊*/
cvRand( &rng, z_k );
cvMatMulAdd( kalman->measurement_matrix, x_k, z_k, z_k );
// Update Kalman filter state
cvKalmanCorrect( kalman, z_k );
// Apply the transition matrix F and apply "process noise" w_k
cvRandSetRange( &rng, 0, sqrt( kalman->process_noise_cov->data.fl[0] ), 0 );/*設置正態分佈過程雜訊*/
cvRand( &rng, w_k );
cvMatMulAdd( kalman->transition_matrix, x_k, w_k, x_k );
// Plot Points
cvZero( img );/*創建圖像*/
// Yellow is observed state 黃色是觀測值
//cvCircle(IntPtr, Point, Int32, MCvScalar, Int32, LINE_TYPE, Int32)
//對應於下列其中，shift為資料精度
//cvCircle(img, center, radius, color, thickness, lineType, shift)
//繪製或填充一個給定圓心和半徑的圓
cvCircle( img,
cvPoint( cvRound(img->width/2 + img->width/3*cos(z_k->data.fl[0])),
cvRound( img->height/2 - img->width/3*sin(z_k->data.fl[0])) ),
4, yellow );
// White is the predicted state via the filter
cvCircle( img,
cvPoint( cvRound(img->width/2 + img->width/3*cos(y_k->data.fl[0])),
cvRound( img->height/2 - img->width/3*sin(y_k->data.fl[0])) ),
4, white, 2 );
// Red is the real state
cvCircle( img,
cvPoint( cvRound(img->width/2 + img->width/3*cos(x_k->data.fl[0])),
cvRound( img->height/2 - img->width/3*sin(x_k->data.fl[0])) ),
4, red );
CvFont font;
cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,0.5f ,0.5f ,0,1,8);
cvPutText(img,"Yellow:observe",cvPoint(0,20),&font,cvScalar(0,0,255));
cvPutText(img,"While:predict",cvPoint(0,40),&font,cvScalar(0,0,255));
cvPutText(img,"Red:real",cvPoint(0,60),&font,cvScalar(0,0,255));
cvPutText(img,"Press Esc to Exit...",cvPoint(0,80),&font,cvScalar(255,255,255));
cvShowImage( "Kalman", img );
// Exit on esc key
if(cvWaitKey(100) == 27)
break;
}
cvReleaseImage(&img);/*釋放圖像*/
cvReleaseKalman(&kalman);/*釋放kalman濾波對象*/
cvDestroyAllWindows();/*釋放所有視窗*/
}
參考網址 ：
留言
張貼留言