34 #include <tf/transform_broadcaster.h>
35 #include <nav_msgs/Odometry.h>
36 #include <kfilter/ekfilter.hpp>
38 #define DEFAULT_WHEEL_BASE 2.5
42 #include <atlascar_base/AtlascarStatus.h>
43 #include <atlascar_base/AtlascarVelocityStatus.h>
46 using namespace Kalman;
61 memset(_P0,0,
sizeof(_P0));
78 cout<<
"P0 "<<P0<<endl;
79 cout<<
"x_0 "<<x_0<<endl;
111 A(1,3)=-sin(yaw)*cos(phi)*vl*dt;
112 A(1,4)=cos(yaw)*cos(phi)*dt;
113 A(1,5)=cos(yaw)*(-sin(phi))*vl*dt;
118 A(2,3)=cos(yaw)*cos(phi)*vl*dt;
119 A(2,4)=sin(yaw)*cos(phi)*dt;
120 A(2,5)=sin(yaw)*(-sin(phi))*vl*dt;
126 A(3,4)=sin(phi)*(dt/l);
128 A(3,5)=cos(phi)*vl*dt/l;
266 x_(1) = x(1) + cos(x(3))*cos(x(5))*x(4)*dt;
267 x_(2) = x(2) + sin(x(3))*cos(x(5))*x(4)*dt;
269 x_(3) = x(3) + sin(x(5))*(x(4)/l)*dt;
void IncommingDataHandler(int)
void SetWheelBase(double l_)
non_holonomic_ekfilter::Matrix Matrix
void SetTimeInterval(double dt_)
non_holonomic_ekfilter::Vector Vector