卡尔曼滤波
@(机器学习)[状态方程|观测方程]
卡尔曼滤波简介
如果你要测量的东西很容易测准确，没有什么随机干扰，那就不需要劳驾卡尔曼。有的时候，我们的测量因为随机干扰，无法准确得到，卡尔曼就给我们想了个办法，让我们在干扰为高斯分布的情况下，得到的测量均方误差最小，也就是测量值扰动最小，看起来最平滑1。
卡尔曼滤波基本公式
滤波估计方程（ K 时刻的最优值）：
xk=xk/k−1+Bk−1uk−1+Jk−1Zk−1+Hk[Zk−CK∗xk/k−1−Ck∗Bk−1uk−1] - xk 为 k 时刻最优值，
xk/k−1 为根据 k−1 时刻进行预测得到的 k 时刻的预测值，由状态一步预测方程得到；uk−1 是控制量， Bk−1 为 k−1 时刻控制系数矩阵- Jk−1Zk−1 分别是过程激励噪声，可以简单理解为噪声
-
状态一步预测方程
xk/k−1=Ak,k−1xk−1+Bk−1uk−1+Jk−1Zk−1- uk−1 是控制量
均方误差一步预测：
Pk/k−1=Ak,k−1∗Pk−1∗ATk,k−1+Tk−1Qk−1TTk−1−Jk−1STk−1TTk−1- Tk−1Qk−1TTk−1−Jk−1STk−1TTk−1 为观测噪声，
- Qk 为过程噪声的协方差，其为非负定矩阵；
均方误差更新矩阵（K时刻最优均方误差）：
Pk=[I−Hk∗Ck]∗Pk/k−1滤波增益方程（权重系数更新）：
Hk=Pk/k−1∗CTk[Ck∗Pk/k−1∗CTk+Rk]−1
其中， Rk 为测量噪声的协方差，其为正定矩阵；
opencv源码剖析
头文件中类声明如下：
class CV_EXPORTS_W KalmanFilter
{
public:
CV_WRAP KalmanFilter();
/** @overload
@param dynamParams Dimensionality of the state.
@param measureParams Dimensionality of the measurement.
@param controlParams Dimensionality of the control vector.
@param type Type of the created matrices that should be CV_32F or CV_64F.
*/
CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );
/** @brief Re-initializes Kalman filter. The previous content is destroyed.
@param dynamParams Dimensionality of the state.
@param measureParams Dimensionality of the measurement.
@param controlParams Dimensionality of the control vector.
@param type Type of the created matrices that should be CV_32F or CV_64F.
*/
void init( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );
/** @brief Computes a predicted state.
@param control The optional input control
*/
CV_WRAP const Mat& predict( const Mat& control = Mat() );
/** @brief Updates the predicted state from the measurement.
@param measurement The measured system parameters
*/
CV_WRAP const Mat& correct( const Mat& measurement );
CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A)
CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control)
CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H)
CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q)
CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
// temporary matrices
Mat temp1;
Mat temp2;
Mat temp3;
Mat temp4;
Mat temp5;
};源文件中类定义如下：
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)
{
CV_INSTRUMENT_REGION()
// update the state: x'(k) = A*x(k)
statePre = transitionMatrix*statePost;
if( !control.empty() )
// 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)
{
CV_INSTRUMENT_REGION()
// 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;
}
}算法应用实例-目标跟踪
static inline Point calcPoint(Point2f center, double R, double angle)
{
return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}
// plot points
static inline void drawCross(cv::Mat& img, Point& center, Color color, float d )
{
cv::line(img, Point( center.x - d, center.y - d ),Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0);
cv::line(img, Point( center.x + d, center.y - d ),Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 );
}
int kalmanTracking()
{
Mat img(500, 500, CV_8UC3);
/*KalmanFilter (int dynamParams, int measureParams, int controlParams=0, int type=CV_32F)
*dynamParams : 状态转移参数
*measureParams : 测量参数
*controlParams : 运动控制参数
*/
KalmanFilter KF(2, 1, 0); //dynamParams:2 measureParams:1
Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
Mat processNoise(2, 1, CV_32F);
Mat measurement = Mat::zeros(1, 1, CV_32F); //测量值
char code = (char)-1;
//初始化状态
randn(state, Scalar::all(0), Scalar::all(0.1));
//初始化kalman参数
KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(KF.errorCovPost, Scalar::all(1));
randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
for (;;)
{
Point2f center(img.cols*0.5f, img.rows*0.5f);
float R = img.cols / 3.f;
double stateAngle = state.at<float>(0);
Point statePt = calcPoint(center, R, stateAngle);
//predict angle
Mat prediction = KF.predict(); //Computes a predicted state.
double predictAngle = prediction.at<float>(0);
Point predictPt = calcPoint(center, R, predictAngle);
randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));
// generate measurement
measurement += KF.measurementMatrix*state;
double measAngle = measurement.at<float>(0);
Point measPt = calcPoint(center, R, measAngle);
img = Scalar::all(0);
drawCross(statePt, Scalar(255, 255, 255), 3);
drawCross(measPt, Scalar(0, 0, 255), 3);
drawCross(predictPt, Scalar(0, 255, 0), 3);
line(img, statePt, measPt, Scalar(0, 0, 255), 3, LINE_AA, 0);
line(img, statePt, predictPt, Scalar(0, 255, 255), 3, LINE_AA, 0);
if (theRNG().uniform(0, 4) != 0)
KF.correct(measurement);
randn(processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
state = KF.transitionMatrix*state + processNoise;
imshow("Kalman", img);
code = (char)waitKey(100);
if (code > 0)
break;
}
//if (code == 27 || code == 'q' || code == 'Q')
//break;
//}
return 0;
}
扫一扫
2190
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
