Commit 89317acf authored by Tadej Borovšak's avatar Tadej Borovšak
Browse files

Add RW and NCA models

parent 845ac022
......@@ -3,7 +3,6 @@ CXXFLAGS := $(shell pkg-config --cflags opencv) -Wall -Wextra -ggdb
LDFLAGS := $(shell pkg-config --libs opencv) -Wall -Wextra -ggdb
APPS := \
test \
kalman
all: $(APPS)
......
......@@ -55,27 +55,75 @@ gen_point (float pos)
}
/* Various settings for Kalman */
static void
init_kalman_rw (KalmanFilter &kf,
Point const &start)
{
kf.init (4, 2, 0);
setIdentity (kf.transitionMatrix);
kf.processNoiseCov = (Mat_<float> (4, 4) <<
1.000, 0.000, 0.000, 0.000,
0.000, 1.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000);
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
setIdentity (kf.errorCovPost, Scalar::all (1));
kf.statePost = (Mat_<float> (4, 1) << start.x, start.y, 0, 0);
}
static void
init_kalman_ncv (KalmanFilter &kf,
Point const &start)
{
kf.init (4, 2, 0);
kf.transitionMatrix = (Mat_<float> (4, 4) << 1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1);
kf.processNoiseCov = (Mat_<float> (4, 4) << 0.333, 0.000, 0.500, 0.000,
0.000, 0.333, 0.000, 0.500,
0.500, 0.000, 1.000, 0.000,
0.000, 0.500, 0.000, 1.000);
kf.transitionMatrix = (Mat_<float> (4, 4) <<
1, 0, 1, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1);
kf.processNoiseCov = (Mat_<float> (4, 4) <<
0.333, 0.000, 0.500, 0.000,
0.000, 0.333, 0.000, 0.500,
0.500, 0.000, 1.000, 0.000,
0.000, 0.500, 0.000, 1.000);
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (100));
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
setIdentity (kf.errorCovPost, Scalar::all (1));
kf.statePost = (Mat_<float> (4, 1) << start.x, start.y, 0, 0);
}
static void
init_kalman_nca (KalmanFilter &kf,
Point const &start)
{
kf.init (6, 2, 0);
kf.transitionMatrix = (Mat_<float> (6, 6) <<
1.0, 0.0, 1.0, 0.0, 0.5, 0.0,
0.0, 1.0, 0.0, 1.0, 0.0, 0.5,
0.0, 0.0, 1.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 1.0,
0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
kf.processNoiseCov = (Mat_<float> (6, 6) <<
0.050, 0.000, 0.125, 0.000, 0.167, 0.000,
0.000, 0.050, 0.000, 0.125, 0.000, 0.167,
0.125, 0.000, 0.333, 0.000, 0.500, 0.000,
0.000, 0.125, 0.000, 0.333, 0.000, 0.500,
0.167, 0.000, 0.500, 0.000, 1.000, 0.000,
0.000, 0.167, 0.000, 0.500, 0.000, 1.000);
setIdentity (kf.measurementMatrix);
setIdentity (kf.measurementNoiseCov, Scalar::all (10));
setIdentity (kf.errorCovPost, Scalar::all (1));
kf.statePost = (Mat_<float> (6, 1) << start.x, start.y, 0, 0, 0, 0);
}
/* Gui helpers */
static inline void
draw_cross (Mat &img,
......@@ -142,7 +190,9 @@ main (int ,
};
init_window (&data);
init_kalman_ncv (data.kf, gen_point (0));
//init_kalman_rw (data.kf, gen_point (0));
//init_kalman_ncv (data.kf, gen_point (0));
init_kalman_nca (data.kf, gen_point (0));
/* Wait for ESC key */
while ((char)waitKey () != 27)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment