Example of Kalman Filter

Because Kalman filtering is used in the study, this is a very hard-to-understand control theory. It took me a long time to understand some basic concepts. Although opencv provides an example, however, this example is based on C, which is hard to understand and reuse. Later I sorted out a simple class, during the Forum, a handsome & romantic french guy was studying the filter. Later I gave him the program and sent it to me after modification, therefore, the code here also contains some of its parts, which are considered 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;
}