Example of kalman filter [reprinted]

It is difficult to understand the control theory. It takes me a long time to understand some basic concepts. Although opencv provides an example, this example is based on C, it was hard to understand and reuse. Later I sorted out a simple class. During the forum, a French guy with handsome & Romantic was also studying this filter, later, I gave him the program and sent it to me after modification, so the code here also contains his part, which is a Sino-French Cooperation product.

For beginners of Kalman, it is really tiring to read textbooks for people like me who do not have any math skills. To be honest, I think the books on basic theories of foreigners are very much appreciated, unlike professors in China,

For beginners, refer

The http://bbs.matwav.com/index.jsp Research Forum has several easy-to-understand Chinese interpretations

Http://www.cs.unc.edu /~ Welch/Kalman/here is a comprehensive Kalman base for foreigners, which is very good.

Sample Code:

==================================== Kalman. h ======================================

// Kalman. h: interface for the Kalman class.
//
//////////////////////////////////////// //////////////////////////////

# If! Defined (afx_kalman_h1_ed3d740f_01d2_4616_8b74_8bf57636f2c01_included _)
# Define afx_kalman_h1_ed3d740f_01d2_4616_8b74_8bf57636f2c01_included _

# If _ MSC_VER> 1000
# Pragma once
# Endif // _ MSC_VER> 1000

# Include <math. h>
# Include "cv. h"

Class kalman
{
Public:
Void init_kalman (int x, int xv, int y, int yv );
CvKalman * cvkalman;
CvMat * state;
CvMat * process_noise;
CvMat * measurement;
Const CvMat * prediction;
CvPoint2D32f get_predict (float x, float y );
Kalman (int x = 0, int xv = 0, int y = 0, int yv = 0 );
// Virtual ~ Kalman ();

};

# Endif //! Defined (afx_kalman_h1_ed3d740f_01d2_4616_8b74_8bf57636f2c01_included _)

==================================== Kalman. cpp ====================================

# Include "kalman. h"
# Include <stdio. h>

/* Tester de printer toutes les valeurs des vecteurs ...*/
/* Tester de changer les matrices du noises */
/* Replace state by cvkalman-> state_post ??? */

CvRandState rng;
Const double T = 0.1;
Kalman: kalman (int x, int xv, int y, int yv)
{
Cvkalman = cvCreateKalman (4, 4, 0 );
State = cvCreateMat (4, 1, CV_32FC1 );
Process_noise = cvCreateMat (4, 1, CV_32FC1 );
Measurement = cvCreateMat (4, 1, CV_32FC1 );
Int code =-1;

/* Create matrix data */
Const float A [] = {
1, T, 0, 0,
0, 1, 0, 0,
0, 0, 1, t,
0, 0, 0, 1
};

Const float H [] = {
1, 0, 0, 0,
0, 0, 0, 0,
0, 0, 1, 0,
0, 0, 0, 0
};

Const 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 (), POW ()/t,
0, 0, POW ()/t, POW ()/POW (T, 2)
};

Const 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
};

Const float R [] = {
1, 0, 0, 0,
0, 0, 0, 0,
0, 0, 1, 0,
0, 0, 0, 0
};


Cvrandinit (& RNG, 0, 1,-1, cv_rand_uni );

Cvzero (measurement );

Cvrandsetrange (& RNG, 0, 0.1, 0 );
Rng. disttype = CV_RAND_NORMAL;

CvRand (& rng, state );

Memcpy (cvkalman-> transition_matrix-> data. fl, A, sizeof ());
Memcpy (cvkalman-> measurement_matrix-> data. fl, H, sizeof (H ));
Memcpy (cvkalman-> process_noise_cov-> data. fl, Q, sizeof (Q ));
Memcpy (cvkalman-> error_cov_post-> data. fl, P, sizeof (P ));
Memcpy (cvkalman-> measurement_noise_cov-> data. fl, R, sizeof (R ));
// CvSetIdentity (cvkalman-> process_noise_cov, cvRealScalar (1e-5 ));
// CvSetIdentity (cvkalman-> error_cov_post, cvRealScalar (1 ));
// CvSetIdentity (cvkalman-> measurement_noise_cov, cvRealScalar (1e-1 ));

/* Choose initial state */

State-> data. fl [0] = x;
State-> data. fl [1] = xv;
State-> data. fl [2] = y;
State-> data. fl [3] = yv;
Cvkalman-> state_post-> data. fl [0] = x;
Cvkalman-> state_post-> data. fl [1] = xv;
Cvkalman-> state_post-> data. fl [2] = y;
Cvkalman-> state_post-> data. fl [3] = yv;

CvRandSetRange (& rng, 0, sqrt (cvkalman-> process_noise_cov-> data. fl [0]), 0 );
CvRand (& rng, process_noise );

}

CvPoint2D32f kalman: get_predict (float x, float y ){

/* Update state with current position */
State-> data. fl [0] = x;
State-> data. fl [2] = y;

/* Predict point position */
/* X 'K = A k + B k
P 'k = A K-1 * AT + Q */
CvRandSetRange (& rng, 0, sqrt (cvkalman-> measurement_noise_cov-> data. fl [0]), 0 );
CvRand (& rng, measurement );

/* Xk =? XK-1 + B? Uk + wk */
CvMatMulAdd (cvkalman-> transition_matrix, state, process_noise, cvkalman-> state_post );

/* Zk = H? Xk + vk */
CvMatMulAdd (cvkalman-> measurement_matrix, cvkalman-> state_post, measurement, measurement );

/* Adjust Kalman filter state */
/* Kk = P' k then T done? H Branch 'K branch T + R)-1
Xk = x' k + Kk okay? Zk-H comment 'K)
Pk = (I-Kk success) Comment 'K */
CvKalmanCorrect (cvkalman, measurement );
Float measured_value_x = measurement-> data. fl [0];
Float measured_value_y = measurement-> data. fl [2];

Const CvMat * prediction = cvKalmanPredict (cvkalman, 0 );
Float predict_value_x = prediction-> data. fl [0];
Float predict_value_y = prediction-> data. fl [2];

Return (cvPoint2D32f (predict_value_x, predict_value_y ));
}

Void kalman: init_kalman (int x, int xv, int y, int yv)
{
State-> data. fl [0] = x;
State-> data. fl [1] = xv;
State-> data. fl [2] = y;
State-> data. fl [3] = yv;
Cvkalman-> state_post-> data. fl [0] = x;
Cvkalman-> state_post-> data. fl [1] = xv;
Cvkalman-> state_post-> data. fl [2] = y;
Cvkalman-> state_post-> data. fl [3] = yv;
}