36 setDim(6, 0, 6, 2, 2);
45 memset(_P0,0,
sizeof(_P0));
82 A(1,3)=cos(t)*cos(f)*
dt;
83 A(1,4)=-sin(t)*cos(f)*v1*
dt;
84 A(1,5)=cos(t)*(-sin(f))*v1*dt;
89 A(2,3)=sin(t)*cos(f)*
dt;
90 A(2,4)=cos(t)*cos(f)*v1*
dt;
91 A(2,5)=sin(t)*(-sin(f))*v1*dt;
105 A(4,5)=cos(f/
l)/
l*v1*
dt;
196 x_(1) = x(1) + cos(x(4))*cos(x(5))*x(3)*
dt;
197 x_(2) = x(2) + sin(x(4))*cos(x(5))*x(3)*
dt;
199 x_(4) = x(4) + sin(x(5)/
l)*x(3)*
dt;
203 x_(5) = x(5) + x(6)*
dt*0.1;
constant_velocity_nh(double _l, double _dt, scan_matching_method _method)
Constructor.
double dt
Iteration interval.
void makeA()
Make the process Jacobian matrix.
void updateDt(double _dt)
Update the iteration interval.
Nonholonomic motion model declaration.
void makeMeasure()
Make measurement, used when measurement is not possible (i'm not using it now)
double l
Wheel base of the nonholonomic vehicle.
void makeQ()
Make process noise covariance matrix.
void makeH()
Make measurement sensitivity matrix.
void makeR()
Make measurement noise covariance matrix.
constant_velocity_nh::Vector Vector
constant_velocity_nh::Matrix Matrix
void makeProcess()
Make process, model iteration.
void makeW()
Make process noise sensitivity matrix.
scan_matching_method method
Scan matching method used, this will influence the errors.
void makeV()
Make measurement noise sensitivity matrix.