Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_pose_estimation/system/ground_vehicle_model.h>
00030 #include <hector_pose_estimation/pose_estimation.h>
00031 #include <hector_pose_estimation/filter/set_filter.h>
00032
00033 #include <limits>
00034
00035 namespace hector_pose_estimation {
00036
00037 template class System_<GroundVehicleModel>;
00038
00039 GroundVehicleModel::GroundVehicleModel()
00040 {
00041 gain_ = 1.0;
00042 base_height_ = 0.0;
00043 min_height_ = -std::numeric_limits<double>::quiet_NaN();
00044 max_height_ = std::numeric_limits<double>::quiet_NaN();
00045
00046 parameters().add("gain", gain_);
00047 parameters().add("base_height", base_height_);
00048 parameters().add("min_height", min_height_);
00049 parameters().add("max_height", max_height_);
00050
00051
00052 dR3 << 0.0, 1.0, 0.0,
00053 -1.0, 0.0, 0.0,
00054 0.0, 0.0, 0.0;
00055 }
00056
00057 GroundVehicleModel::~GroundVehicleModel()
00058 {
00059 }
00060
00061 void GroundVehicleModel::getPrior(State &state)
00062 {
00063 GenericQuaternionSystemModel::getPrior(state);
00064 if (state.position()) state.position()->vector().z() = base_height_;
00065 }
00066
00067 void GroundVehicleModel::getDerivative(StateVector& x_dot, const State& state)
00068 {
00069
00070 GenericQuaternionSystemModel::getDerivative(x_dot, state);
00071
00072 const State::RotationMatrix &R = state.R();
00073 State::ConstVelocityType v(state.getVelocity());
00074
00075
00076 if (state.velocity()) {
00077
00078 state.velocity()->segment(x_dot) += -gain_ * R.col(2) * (R.col(2).dot(v));
00079 }
00080 }
00081
00082 void GroundVehicleModel::getStateJacobian(SystemMatrix& A, const State& state, bool init)
00083 {
00084 GenericQuaternionSystemModel::getStateJacobian(A, state, init);
00085
00086 const State::RotationMatrix &R = state.R();
00087 State::ConstVelocityType v(state.getVelocity());
00088
00089 if (state.velocity()) {
00090 state.velocity()->block(A) += -gain_ * R.col(2) * R.col(2).transpose();
00091
00092 if (state.orientation()) {
00093 state.velocity()->block(A, *state.orientation()) += -gain_ * (dR3 * (R.col(2).dot(v)) + R.col(2) * (v.transpose() * dR3));
00094 }
00095 }
00096 }
00097
00098 SystemStatus GroundVehicleModel::getStatusFlags(const State& state)
00099 {
00100 SystemStatus flags = GenericQuaternionSystemModel::getStatusFlags(state);
00101 if (flags & STATE_VELOCITY_XY) {
00102 flags |= STATE_VELOCITY_Z;
00103 flags |= STATE_POSITION_Z;
00104 }
00105 return flags;
00106 }
00107
00108 bool GroundVehicleModel::limitState(State& state)
00109 {
00110 bool result = GenericQuaternionSystemModel::limitState(state);
00111 if (state.position()) {
00112 if (state.position()->vector().z() < min_height_) {
00113 state.position()->vector().z() = min_height_;
00114 result = false;
00115 }
00116 if (state.position()->vector().z() > max_height_) {
00117 state.position()->vector().z() = max_height_;
00118 result = false;
00119 }
00120 }
00121 return result;
00122 }
00123
00124 }