CvKalman

本文用一些具體的代碼來描述Kalman 濾波器狀態，並說明結構 CvKalman 用來保存 Kalman 濾波器狀態及其參數。

基本介紹

• 中文名：CvKalman
• 外文名：typedef struct 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) 在每一步中都被更新。