在視訊跟蹤處理中，預測目標運動軌跡是一項基本任務。目標運動狀態估計的目的有三個：一是對目標過去的狀態進行平滑；二是對目標現在的運動狀態進行濾波；三是對目標未來的運動狀態進行預測。物體的運動狀態一般包括目標位置、速度、加速度等。著名的Kalman濾波技術就是其中一種，這是一種線性系統估計技術。

OpenCV中自帶了kalman濾波的程式碼和例程，可參照kalman.cpp，它存在於類KalmanFilter中。kalman濾波演算法的呼叫比較方便，主要的難點是瞭解多個引數和矩陣計算公式。一個總體的思路是，需要了解前一時刻的狀態估計值x和當前的觀測值y，然後建立狀態方程和觀測方程。經過一些運算後即可預測下一步的狀態。

一、離散時間線性動態系統的狀態方程

Kalman濾波利用線性系統狀態方程，通過系統輸入輸出觀測資料，對系統狀態進行最優估計的演算法。由於觀測資料中包括系統中的噪聲和干擾的影響，所以最優估計也可看作是濾波過程。一個線性系統是採用狀態方程、觀測方程及其初始條件來描述。線性離散時間系統的一般狀態方程可描述為：

　　　　　　　
其中， 是狀態轉移矩陣， 是過程噪聲增益矩陣。是k時刻目標的狀態向量， 是過程噪聲，它是具有均值為零、方差矩陣為Q(k)的高斯噪聲向量，即：

　　　　　　　　
二、感測器的觀測方程

感測器的通用觀測方程為：

　　　
這裡， 是感測器在 時刻的觀測向量，觀測噪聲

　　　　
三、初始狀態的描述
初始狀態 是高斯的，具有均值 和協方差 ，即：

　　　　　　
以上的描述比較抽象，因此記錄一個例子加以說明：

例：目標沿x軸作勻速直線運動，過程噪聲為速度噪聲，試寫出目標的狀態方程。

解：由題意知，目標的狀態為：

用T表示時間間隔，ux錶速度噪聲，則有：

寫成矩陣形式為：

令：

則有：

其中：

為均值等於0，方差為q的高斯噪聲。

在OpenCV中自帶的例程裡面描述了一個一維的運動跟蹤，該點在一個圓弧上運動，只有一個自由度即角度。因此只需建立勻速運動模型即可。

例程的路徑：C:\opencv\sources\samples\cpp\kalman.cpp

在程式碼中各變數的對應情況如下：
狀態估計值X對應：state
當前觀測值Z對應：measurement

KalmanFilter類內成員變數transitionMatrix即為狀態轉移方程中的矩陣A
KalmanFilter類內成員變數measurementMatrix即為量測方程中矩陣C

     Mat statePre;          //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)  
    Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  
    Mat transitionMatrix;   //!< state transition matrix (A)  
    Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)  
    Mat measurementMatrix;  //!< measurement matrix (H)  
    Mat processNoiseCov;    //!< process noise covariance matrix (Q)  
    Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)  
    Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/  
    Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)  
    Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)  

以下是OpenCV/modules/video/src/Kalman.cpp的原始碼，後續需繼續分析這些程式碼：

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"


CV_IMPL CvKalman*
cvCreateKalman( int DP, int MP, int CP )
{
    CvKalman *kalman = 0;

    if( DP <= 0 || MP <= 0 )
        CV_Error( CV_StsOutOfRange,
        "state and measurement vectors must have positive number of dimensions" );

    if( CP < 0 )
        CP = DP;

    /* allocating memory for the structure */
    kalman = (CvKalman *)cvAlloc( sizeof( CvKalman ));
    memset( kalman, 0, sizeof(*kalman));

    kalman->DP = DP;
    kalman->MP = MP;
    kalman->CP = CP;

    kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 );
    cvZero( kalman->state_pre );

    kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 );
    cvZero( kalman->state_post );

    kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 );
    cvSetIdentity( kalman->transition_matrix );

    kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 );
    cvSetIdentity( kalman->process_noise_cov );

    kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 );
    cvZero( kalman->measurement_matrix );

    kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 );
    cvSetIdentity( kalman->measurement_noise_cov );

    kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 );

    kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 );
    cvZero( kalman->error_cov_post );

    kalman->gain = cvCreateMat( DP, MP, CV_32FC1 );

    if( CP > 0 )
    {
        kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 );
        cvZero( kalman->control_matrix );
    }

    kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 );
    kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 );
    kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 );
    kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 );
    kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 );

#if 1
    kalman->PosterState = kalman->state_pre->data.fl;
    kalman->PriorState = kalman->state_post->data.fl;
    kalman->DynamMatr = kalman->transition_matrix->data.fl;
    kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;
    kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;
    kalman->PNCovariance = kalman->process_noise_cov->data.fl;
    kalman->KalmGainMatr = kalman->gain->data.fl;
    kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;
    kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;
#endif

    return kalman;
}


CV_IMPL void
cvReleaseKalman( CvKalman** _kalman )
{
    CvKalman *kalman;

    if( !_kalman )
        CV_Error( CV_StsNullPtr, "" );

    kalman = *_kalman;
    if( !kalman )
        return;

    /* freeing the memory */
    cvReleaseMat( &kalman->state_pre );
    cvReleaseMat( &kalman->state_post );
    cvReleaseMat( &kalman->transition_matrix );
    cvReleaseMat( &kalman->control_matrix );
    cvReleaseMat( &kalman->measurement_matrix );
    cvReleaseMat( &kalman->process_noise_cov );
    cvReleaseMat( &kalman->measurement_noise_cov );
    cvReleaseMat( &kalman->error_cov_pre );
    cvReleaseMat( &kalman->gain );
    cvReleaseMat( &kalman->error_cov_post );
    cvReleaseMat( &kalman->temp1 );
    cvReleaseMat( &kalman->temp2 );
    cvReleaseMat( &kalman->temp3 );
    cvReleaseMat( &kalman->temp4 );
    cvReleaseMat( &kalman->temp5 );

    memset( kalman, 0, sizeof(*kalman));

    /* deallocating the structure */
    cvFree( _kalman );
}


CV_IMPL const CvMat*
cvKalmanPredict( CvKalman* kalman, const CvMat* control )
{
    if( !kalman )
        CV_Error( CV_StsNullPtr, "" );

    /* update the state */
    /* x'(k) = A*x(k) */
    cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre );

    if( control && kalman->CP > 0 )
        /* x'(k) = x'(k) + B*u(k) */
        cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre );

    /* update error covariance matrices */
    /* temp1 = A*P(k) */
    cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 );

    /* P'(k) = temp1*At + Q */
    cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,
                     kalman->error_cov_pre, CV_GEMM_B_T );

    /* handle the case when there will be measurement before the next predict */
    cvCopy(kalman->state_pre, kalman->state_post);

    return kalman->state_pre;
}


CV_IMPL const CvMat*
cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )
{
    if( !kalman || !measurement )
        CV_Error( CV_StsNullPtr, "" );

    /* temp2 = H*P'(k) */
    cvMatMulAdd( kalman->measurement_matrix, kalman->error_cov_pre, 0, kalman->temp2 );
    /* temp3 = temp2*Ht + R */
    cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,
            kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T );

    /* temp4 = inv(temp3)*temp2 = Kt(k) */
    cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD );

    /* K(k) */
    cvTranspose( kalman->temp4, kalman->gain );

    /* temp5 = z(k) - H*x'(k) */
    cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 );

    /* x(k) = x'(k) + K(k)*temp5 */
    cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post );

    /* P(k) = P'(k) - K(k)*temp2 */
    cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,
                     kalman->error_cov_post, 0 );

    return kalman->state_post;
}

namespace cv
{

KalmanFilter::KalmanFilter() {}
KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
{
    init(dynamParams, measureParams, controlParams, type);
}

void KalmanFilter::init(int DP, int MP, int CP, int type)
{
    CV_Assert( DP > 0 && MP > 0 );
    CV_Assert( type == CV_32F || type == CV_64F );
    CP = std::max(CP, 0);

    statePre = Mat::zeros(DP, 1, type);
    statePost = Mat::zeros(DP, 1, type);
    transitionMatrix = Mat::eye(DP, DP, type);

    processNoiseCov = Mat::eye(DP, DP, type);
    measurementMatrix = Mat::zeros(MP, DP, type);
    measurementNoiseCov = Mat::eye(MP, MP, type);

    errorCovPre = Mat::zeros(DP, DP, type);
    errorCovPost = Mat::zeros(DP, DP, type);
    gain = Mat::zeros(DP, MP, type);

    if( CP > 0 )
        controlMatrix = Mat::zeros(DP, CP, type);
    else
        controlMatrix.release();

    temp1.create(DP, DP, type);
    temp2.create(MP, DP, type);
    temp3.create(MP, MP, type);
    temp4.create(MP, DP, type);
    temp5.create(MP, 1, type);
}

const Mat& KalmanFilter::predict(const Mat& control)
{
    // update the state: x'(k) = A*x(k)
    statePre = transitionMatrix*statePost;

    if( control.data )
        // x'(k) = x'(k) + B*u(k)
        statePre += controlMatrix*control;

    // update error covariance matrices: temp1 = A*P(k)
    temp1 = transitionMatrix*errorCovPost;

    // P'(k) = temp1*At + Q
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);

    // handle the case when there will be measurement before the next predict.
    statePre.copyTo(statePost);
    errorCovPre.copyTo(errorCovPost);

    return statePre;
}

const Mat& KalmanFilter::correct(const Mat& measurement)
{
    // temp2 = H*P'(k)
    temp2 = measurementMatrix * errorCovPre;

    // temp3 = temp2*Ht + R
    gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);

    // temp4 = inv(temp3)*temp2 = Kt(k)
    solve(temp3, temp2, temp4, DECOMP_SVD);

    // K(k)
    gain = temp4.t();

    // temp5 = z(k) - H*x'(k)
    temp5 = measurement - measurementMatrix*statePre;

    // x(k) = x'(k) + K(k)*temp5
    statePost = statePre + gain*temp5;

    // P(k) = P'(k) - K(k)*temp2
    errorCovPost = errorCovPre - gain*temp2;

    return statePost;
}

}