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/generic_quaternion_system_model.h>
00030 #include <hector_pose_estimation/filter/set_filter.h>
00031
00032 #include <hector_pose_estimation/pose_estimation.h>
00033
00034 namespace hector_pose_estimation {
00035
00036 template class System_<GenericQuaternionSystemModel>;
00037
00038 GenericQuaternionSystemModel::GenericQuaternionSystemModel()
00039 {
00040 angular_acceleration_stddev_ = 360.0 * M_PI/180.0;
00041 rate_stddev_ = 0.0;
00042 acceleration_stddev_ = 0.0;
00043 velocity_stddev_ = 0.0;
00044
00045
00046 parameters().add("angular_acceleration_stddev", angular_acceleration_stddev_);
00047 parameters().add("rate_stddev", rate_stddev_);
00048 parameters().add("acceleration_stddev", acceleration_stddev_);
00049 parameters().add("velocity_stddev", velocity_stddev_);
00050 }
00051
00052 GenericQuaternionSystemModel::~GenericQuaternionSystemModel()
00053 {
00054 }
00055
00056 bool GenericQuaternionSystemModel::init(PoseEstimation& estimator, System &system, State& state)
00057 {
00058 gravity_ = estimator.parameters().get("gravity_magnitude");
00059
00060 imu_ = estimator.getInputType<ImuInput>("imu");
00061 if (imu_ && state.orientation()) {
00062 gyro_ = estimator.getSystem_<Gyro>("gyro");
00063 if (!gyro_) {
00064 gyro_.reset(new Gyro("gyro"));
00065 estimator.addSystem(gyro_);
00066 }
00067 }
00068 if (imu_ && state.velocity()) {
00069 accelerometer_ = estimator.getSystem_<Accelerometer>("accelerometer");
00070 if (!accelerometer_) {
00071 accelerometer_.reset(new Accelerometer("accelerometer"));
00072 estimator.addSystem(accelerometer_);
00073 }
00074
00075 }
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 return true;
00100 }
00101
00102 void GenericQuaternionSystemModel::getPrior(State &state) {
00103 if (state.orientation()) {
00104 state.orientation()->P()(X,X) = 1.0;
00105 state.orientation()->P()(Y,Y) = 1.0;
00106 state.orientation()->P()(Z,Z) = 0.0;
00107 }
00108
00109 if (state.rate()) {
00110 state.rate()->P()(X,X) = pow(0.0 * M_PI/180.0, 2);
00111 state.rate()->P()(Y,Y) = pow(0.0 * M_PI/180.0, 2);
00112 state.rate()->P()(Z,Z) = pow(0.0 * M_PI/180.0, 2);
00113 }
00114
00115 if (state.position()) {
00116 state.position()->P()(X,X) = 0.0;
00117 state.position()->P()(Y,Y) = 0.0;
00118 state.position()->P()(Z,Z) = 0.0;
00119 }
00120
00121 if (state.velocity()) {
00122 state.velocity()->P()(X,X) = 0.0;
00123 state.velocity()->P()(Y,Y) = 0.0;
00124 state.velocity()->P()(Z,Z) = 0.0;
00125 }
00126 }
00127
00128 bool GenericQuaternionSystemModel::prepareUpdate(State& state, double dt)
00129 {
00130 if (state.rate()) {
00131 rate_nav_ = state.R() * state.getRate();
00132 }
00133 else if (rate_input_) {
00134 rate_nav_ = state.R() * rate_input_->getVector();
00135 }
00136 else if (imu_) {
00137 if (gyro_) {
00138 rate_nav_ = state.R() * gyro_->getModel()->getRate(imu_->getRate(), state);
00139 } else {
00140 rate_nav_ = state.R() * imu_->getRate();
00141 }
00142 } else {
00143 rate_nav_.setZero();
00144 }
00145
00146 if (state.acceleration()) {
00147 acceleration_nav_ = state.R() * state.getAcceleration();
00148 }
00149 else if (force_input_) {
00150 acceleration_nav_ = state.R() * force_input_->getVector();
00151 }
00152 else if (imu_) {
00153 if (accelerometer_) {
00154 acceleration_nav_ = state.R() * accelerometer_->getModel()->getAcceleration(imu_->getAcceleration(), state);
00155 } else {
00156 acceleration_nav_ = state.R() * imu_->getAcceleration();
00157 }
00158 } else {
00159 acceleration_nav_.setZero();
00160 }
00161
00162 ROS_DEBUG_STREAM_NAMED("system", "rate_nav = [" << rate_nav_.transpose() << "]");
00163 ROS_DEBUG_STREAM_NAMED("system", "acceleration_nav = [" << acceleration_nav_.transpose() << "]");
00164 return true;
00165 }
00166
00167 void GenericQuaternionSystemModel::getDerivative(StateVector& x_dot, const State& state)
00168 {
00169 x_dot.setZero();
00170
00171 if (state.rate()) {
00172 if (torque_input_) {
00173 state.rate()->segment(x_dot) = torque_input_->getVector();
00174 }
00175 }
00176
00177 if (state.orientation()) {
00178 state.orientation()->segment(x_dot).head(3) = rate_nav_;
00179 if (!(state.getSystemStatus() & STATE_YAW) || (state.getSystemStatus() & STATUS_ALIGNMENT)) {
00180 state.orientation()->segment(x_dot).z() = 0.0;
00181 }
00182 }
00183
00184 if (state.velocity()) {
00185 if ((state.getSystemStatus() & STATE_VELOCITY_XY) && !(state.getSystemStatus() & STATUS_ALIGNMENT)) {
00186 state.velocity()->segment(x_dot)(X) = acceleration_nav_.x();
00187 state.velocity()->segment(x_dot)(Y) = acceleration_nav_.y();
00188 }
00189 if ((state.getSystemStatus() & STATE_VELOCITY_Z) && !(state.getSystemStatus() & STATUS_ALIGNMENT)) {
00190 state.velocity()->segment(x_dot)(Z) = acceleration_nav_.z();
00191 if (imu_) {
00192 state.velocity()->segment(x_dot)(Z) += gravity_;
00193 }
00194 }
00195 }
00196
00197 if (state.position()) {
00198 State::ConstVelocityType v(state.getVelocity());
00199 if ((state.getSystemStatus() & STATE_POSITION_XY) && !(state.getSystemStatus() & STATUS_ALIGNMENT)) {
00200 state.position()->segment(x_dot)(X) = v.x();
00201 state.position()->segment(x_dot)(Y) = v.y();
00202 }
00203 if ((state.getSystemStatus() & STATE_POSITION_Z) && !(state.getSystemStatus() & STATUS_ALIGNMENT)) {
00204 state.position()->segment(x_dot)(Z) = v.z();
00205 }
00206 }
00207 }
00208
00209 void GenericQuaternionSystemModel::getSystemNoise(NoiseVariance& Q, const State& state, bool init)
00210 {
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240 if (!init) return;
00241
00242 Q.setZero();
00243 if (state.orientation()) {
00244 if (!state.rate() && imu_ && gyro_) {
00245 gyro_->getModel()->getRateNoise(state.orientation()->block(Q), state, init);
00246 }
00247 state.orientation()->block(Q) += pow(rate_stddev_, 2) * SymmetricMatrix3::Identity();
00248 }
00249 if (state.rate()) {
00250 state.rate()->block(Q) = pow(angular_acceleration_stddev_, 2) * SymmetricMatrix3::Identity();
00251 }
00252 if (state.position()) {
00253 state.position()->block(Q) = pow(velocity_stddev_, 2) * SymmetricMatrix3::Identity();
00254 }
00255 if (state.velocity()) {
00256 if (!state.acceleration() && imu_ && accelerometer_) {
00257 accelerometer_->getModel()->getAccelerationNoise(state.velocity()->block(Q), state, init);
00258 }
00259 state.velocity()->block(Q) += pow(acceleration_stddev_, 2) * SymmetricMatrix3::Identity();
00260 }
00261 }
00262
00263 void GenericQuaternionSystemModel::getStateJacobian(SystemMatrix& A, const State& state, bool)
00264 {
00265 const State::RotationMatrix &R = state.R();
00266 A.setZero();
00267
00268 if (state.orientation()) {
00269 if (state.rate()) {
00270 state.orientation()->block(A, *state.rate()) = R;
00271 } else if (imu_ && gyro_) {
00272 GyroModel::SystemMatrixBlock A_orientation = state.orientation()->rows(A);
00273 gyro_->getModel()->getRateJacobian(A_orientation, state);
00274 state.orientation()->rows(A) = R * A_orientation;
00275 }
00276
00277 state.orientation()->block(A) += SkewSymmetricMatrix(-rate_nav_);
00278
00279 if (!(state.getSystemStatus() & STATE_YAW) || (state.getSystemStatus() & STATUS_ALIGNMENT)) {
00280 state.orientation()->rows(A).row(2).setZero();
00281 state.orientation()->block(A).col(2).setZero();
00282 }
00283 }
00284
00285 if (state.velocity()) {
00286 if (state.acceleration()) {
00287 state.velocity()->block(A, *state.acceleration()) = R;
00288 } else if (imu_ && accelerometer_) {
00289 AccelerometerModel::SystemMatrixBlock A_velocity = state.velocity()->rows(A);
00290 accelerometer_->getModel()->getAccelerationJacobian(A_velocity, state);
00291 state.velocity()->rows(A) = R * A_velocity;
00292 }
00293
00294 if (state.orientation()) {
00295 state.velocity()->block(A, *state.orientation()) += SkewSymmetricMatrix(-acceleration_nav_);
00296 }
00297
00298 if (!(state.getSystemStatus() & STATE_VELOCITY_XY) || (state.getSystemStatus() & STATUS_ALIGNMENT)) {
00299 state.velocity()->rows(A).topRows(2).setZero();
00300 }
00301
00302 if (!(state.getSystemStatus() & STATE_VELOCITY_Z) || (state.getSystemStatus() & STATUS_ALIGNMENT)) {
00303 state.velocity()->rows(A).row(2).setZero();
00304 }
00305 }
00306
00307 if (state.position() && state.velocity()) {
00308 state.position()->block(A, *state.velocity()).setIdentity();
00309
00310 if ((state.getSystemStatus() & STATE_POSITION_XY) && !(state.getSystemStatus() & STATUS_ALIGNMENT)) {
00311 state.position()->block(A, *state.velocity())(X,X) = 1.0;
00312 state.position()->block(A, *state.velocity())(Y,Y) = 1.0;
00313 }
00314 if ((state.getSystemStatus() & STATE_POSITION_Z) && !(state.getSystemStatus() & STATUS_ALIGNMENT)) {
00315 state.position()->block(A, *state.velocity())(Z,Z) = 1.0;
00316 }
00317 }
00318
00319 }
00320
00321 SystemStatus GenericQuaternionSystemModel::getStatusFlags(const State& state)
00322 {
00323 SystemStatus flags = state.getMeasurementStatus();
00324 if (flags & STATE_POSITION_XY) flags |= STATE_VELOCITY_XY;
00325 if (flags & STATE_POSITION_Z) flags |= STATE_VELOCITY_Z;
00326 if (imu_) {
00327 if (flags & STATE_VELOCITY_XY) flags |= STATE_ROLLPITCH;
00328 if (flags & STATE_ROLLPITCH) flags |= STATE_RATE_XY;
00329 if (flags & STATE_PSEUDO_ROLLPITCH) flags |= STATE_PSEUDO_RATE_XY;
00330 if (flags & STATE_YAW) flags |= STATE_RATE_Z;
00331 if (flags & STATE_PSEUDO_YAW) flags |= STATE_PSEUDO_RATE_Z;
00332 }
00333 return flags & STATE_MASK;
00334 }
00335
00336 }