33 #error This file should not be used, DEPRECATED
39 for(
int l=0;l<size;l++)
40 for(
int c=0;c<size;c++)
49 for(
int l=0;l<size;l++)
50 for(
int c=0;c<size;c++)
100 P_0(0,1)=P_init(0,1);
101 P_0(0,2)=P_init(0,2);
102 P_0(0,3)=P_init(0,3);
104 P_0(1,0)=P_init(1,0);
105 P_0(1,1)=P_init(1,1);
106 P_0(1,2)=P_init(1,2);
107 P_0(1,3)=P_init(1,3);
109 P_0(2,0)=P_init(2,0);
110 P_0(2,1)=P_init(2,1);
111 P_0(2,2)=P_init(2,2);
112 P_0(2,3)=P_init(2,3);
114 P_0(3,0)=P_init(3,0);
115 P_0(3,1)=P_init(3,1);
116 P_0(3,2)=P_init(3,2);
117 P_0(3,3)=P_init(3,3);
190 double inovation_factor = 3.0;
191 double distortion_y = 0.1;
239 rot << cos(dir), -sin(dir),
242 Matrix2d cov = rot*eigval*rot.inverse();
311 double factor=pow(sigma,2)*
dt/6.;
313 Q(0,0)=factor*2*pow(
dt,2);
314 Q(1,1)=factor*2*pow(
dt,2);
316 Q(0,2)=factor*3*pow(
dt,1);
317 Q(2,1)=factor*3*pow(
dt,1);
319 Q(1,3)=factor*3*pow(
dt,1);
320 Q(3,1)=factor*3*pow(
dt,1);
330 x_(0) = x(0) + x(2)*
dt;
331 x_(1) = x(1) + x(3)*
dt;
void SetIdentity(kMatrix &M, int size, double value=1)
Set a matrix to identity.
void makeBaseV()
Make measurement noise sensitivity matrix.
void makeProcess()
Make process, model iteration.
void makeBaseH()
Make measurement sensitivity matrix.
void makeR()
Make measurement noise covariance matrix.
void SetZero(kMatrix &M, int size)
Set a matrix to zero.
void makeBaseA()
Make the base constant process Jacobian matrix.
double dt
Time interval between measurements.
void InitFilter(Vector4d &x_init)
Init filter.
void makeBaseW()
Make process noise sensitivity matrix.
Time lt
Time of the last call to the makeCommonProcess function.
MHT generic types source code, NOT USED, DEPRECATED.
void makeQ()
Make process noise covariance matrix.
EKFilter< double, 0, false, false, false >::Vector Vector
EKfilter vector type.
void makeA()
Make the process Jacobian matrix.
void makeCommonProcess()
This function is called before all other make something functions.
void makeMeasure()
Make measurement, used when measurement is not possible (i'm not using it now)