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 GroundVehicleModel::~GroundVehicleModel()
00053 {
00054 }
00055
00056 void GroundVehicleModel::getPrior(State &state)
00057 {
00058 GenericQuaternionSystemModel::getPrior(state);
00059 state.position().z() = base_height_;
00060 }
00061
00062 void GroundVehicleModel::getDerivative(StateVector& x_dot, const State& state)
00063 {
00064
00065 GenericQuaternionSystemModel::getDerivative(x_dot, state);
00066
00067
00068 State::ConstVelocityType v(state.getVelocity());
00069
00070
00071
00072 #ifdef VELOCITY_IN_BODY_FRAME
00073 if (state.getVelocityIndex() >= 0) {
00074 x_dot(State::VELOCITY_Z) = -gain_ * v.z();
00075 }
00076
00077 #else
00078 if (state.getVelocityIndex() >= 0) {
00079
00080 x_dot(State::VELOCITY_Z) = -gain_ * R(2,2) * R.row(2).dot(v);
00081 }
00082 #endif // VELOCITY_IN_BODY_FRAME
00083 }
00084
00085 void GroundVehicleModel::getStateJacobian(SystemMatrix& A, const State& state, bool init)
00086 {
00087 GenericQuaternionSystemModel::getStateJacobian(A, state, init);
00088
00089 State::ConstOrientationType q(state.getOrientation());
00090 State::ConstVelocityType v(state.getVelocity());
00091
00092 #ifdef VELOCITY_IN_BODY_FRAME
00093 if (state.getVelocityIndex() >= 0) {
00094 A(State::VELOCITY_Z,State::VELOCITY_Z) = -gain_;
00095 }
00096
00097 #else
00098 if (state.getVelocityIndex() >= 0) {
00099 A.block<1,3>(State::VELOCITY_Z,State::VELOCITY_X) = -gain_ * R(2,2) * R.row(2);
00100
00101 if (state.getOrientationIndex() >= 0) {
00102 dr3_dq_ << 2*q.y(), 2*q.z(), 2*q.w(), 2*q.x(),
00103 -2*q.x(), -2*q.w(), 2*q.z(), 2*q.y(),
00104 2*q.w(), -2*q.x(), -2*q.y(), 2*q.z();
00105
00106 A.block<1,4>(State::VELOCITY_Z,state.getOrientationIndex()) = -gain_ * ((dr3_dq_.row(2) * R.row(2).dot(v)) + R(2,2) * (v.transpose() * dr3_dq_));
00107 }
00108 }
00109 #endif // VELOCITY_IN_BODY_FRAME
00110 }
00111
00112 SystemStatus GroundVehicleModel::getStatusFlags(const State& state)
00113 {
00114 SystemStatus flags = GenericQuaternionSystemModel::getStatusFlags(state);
00115 flags |= STATE_VELOCITY_Z;
00116 flags |= STATE_POSITION_Z;
00117 return flags;
00118 }
00119
00120 bool GroundVehicleModel::limitState(State& state)
00121 {
00122 bool result = GenericQuaternionSystemModel::limitState(state);
00123 if (state.position().z() < min_height_) {
00124 state.position().z() = min_height_;
00125 result = false;
00126 }
00127 if (state.position().z() > max_height_) {
00128 state.position().z() = max_height_;
00129 result = false;
00130 }
00131 return result;
00132 }
00133
00134 }