/home/laradmin/lar/perception/planarobstacles/mtt/src/mtt_kalman.cpp File Reference

Kalman estimation related functions. More...

#include <mtt/mtt_kalman.h>

Include dependency graph for mtt_kalman.cpp:

Go to the source code of this file.

Defines
#define	CV_MODEL   0
Functions
void	AddPointErrorVectors (t_errors *error, double x_inno, double y_inno, double x_resi, double y_resi, double lateral_error)
void	AllocMotionModels (t_list &list, t_config &config)
CvKalman *	CreateModelCTRV (void)
CvKalman *	CreateModelCV (void)
CvKalman *	CreateModelCV_SC (void)
CvKalman *	CreateModelFwdCt (void)
void	dA_FwdCt (CvKalman *model, double q[6], double dt, double l)
void	dH_FwdCt (CvKalman *model)
void	GetErrorConvariance (t_errors *error)
	Get covariance.
void	IterateMotionModelCTRV (CvKalman *model, double vm, double wm)
void	IterateMotionModelCV (CvKalman *model, double vxm, double vym)
double	IterateMotionModelCV_SC (CvKalman *model, double vm)
void	IterateMotionModelFwdCt (CvKalman *model, double z[2])
void	MotionModelsIteration (vector< t_listPtr > &list, t_config &config)
void	UpdateTransitionMatrixCTRV (CvKalman *model, double, double, double t, double v, double w, double dt)
void	UpdateTransitionMatrixCV (CvKalman *model, double dt)
void	UpdateTransitionMatrixCV_SC (CvKalman *model, double dt)
Variables
double	gl
int	select_object

---

Detailed Description

Kalman estimation related functions.

Definition in file mtt_kalman.cpp.

---

Define Documentation

#define CV_MODEL   0

Definition at line 34 of file mtt_kalman.cpp.

---

Function Documentation

void AddPointErrorVectors	(	t_errors *	error,
		double	x_inno,
		double	y_inno,
		double	x_resi,
		double	y_resi,
		double	lateral_error
	)

Definition at line 40 of file mtt_kalman.cpp.

void AllocMotionModels	(	t_list &	list,
		t_config &	config
	)

Definition at line 415 of file mtt_kalman.cpp.

CvKalman* CreateModelCTRV

(

void

)

Definition at line 637 of file mtt_kalman.cpp.

CvKalman* CreateModelCV

(

void

)

Definition at line 705 of file mtt_kalman.cpp.

CvKalman* CreateModelCV_SC

(

void

)

Definition at line 673 of file mtt_kalman.cpp.

CvKalman* CreateModelFwdCt

(

void

)

Definition at line 756 of file mtt_kalman.cpp.

void dA_FwdCt	(	CvKalman *	model,
		double	q[6],
		double	dt,
		double	l
	)

Definition at line 1036 of file mtt_kalman.cpp.

void dH_FwdCt

(

CvKalman *

model

)

Definition at line 1025 of file mtt_kalman.cpp.

void GetErrorConvariance

(

t_errors *

error

)

Get covariance.

Returns:

void

Initalise auxiliar matrixes

Set accumulators to 0

Do Cov(Innovation) = 1/m * S(i=0,i< m-1,d[k-i]*d[k-i]') and Cov(Residue) = 1/m * S(i=0,i< m-1,e[k-i]*e[k-i]')

Todo:

Wend the number of points is growing the covariance is way to big, i don't realy know why

Definition at line 507 of file mtt_kalman.cpp.

void IterateMotionModelCTRV	(	CvKalman *	model,
		double	vm,
		double	wm
	)

Definition at line 779 of file mtt_kalman.cpp.

void IterateMotionModelCV	(	CvKalman *	model,
		double	vxm,
		double	vym
	)

Definition at line 912 of file mtt_kalman.cpp.

double IterateMotionModelCV_SC	(	CvKalman *	model,
		double	vm
	)

Definition at line 873 of file mtt_kalman.cpp.

void IterateMotionModelFwdCt	(	CvKalman *	model,
		double	z[2]
	)

Definition at line 841 of file mtt_kalman.cpp.

void MotionModelsIteration	(	vector< t_listPtr > &	list,
		t_config &	config
	)

If the object is new, we set the prestate and poststate, given that this is not a new measurement, of the filter to the current measurement

Get the new measurement into the filter and correct, t=k

Extract the correct state from the filter and put it to path[k]

After correction iterate the filter to make a prediction, t=k+1

Extract the prediction from the filter

Obtain error vectors

Add estimated point to path

Put velocity into data file

Obtain velocity

Put lateral error into data file

Put error cov and AKF gains into data file

Define a new search area based on the kalman errors and predicted position

Do some tweaks on the kalman errors

* Update white noise scale **************************/

Only update if the object is visible

Definition at line 64 of file mtt_kalman.cpp.

void UpdateTransitionMatrixCTRV	(	CvKalman *	model,
		double	,
		double	,
		double	t,
		double	v,
		double	w,
		double	dt
	)

Definition at line 1155 of file mtt_kalman.cpp.

void UpdateTransitionMatrixCV	(	CvKalman *	model,
		double	dt
	)

Definition at line 974 of file mtt_kalman.cpp.

void UpdateTransitionMatrixCV_SC	(	CvKalman *	model,
		double	dt
	)

Definition at line 954 of file mtt_kalman.cpp.

---

Variable Documentation

double gl

int select_object

Definition at line 36 of file mtt_kalman.cpp.