Exemplo n.º 1
0
  void jacInput(mtJacInput& J, const mtInput& input, const mtMeas& meas, double dt = 0.0) const{
    J.setZero();

    rot::EulerAnglesYprPD ypr(input.template get<mtInput::_att>());
    const double theta = ypr.pitch();
    const double phi = ypr.roll();
    const double t2 = cos(theta);
    const double t3 = 1.0/t2;
    const double t4 = cos(phi);
    const double t5 = sin(phi);
    const double t6 = sin(theta);

    J(0,0) = 0.0;
    J(0,1) = t3*t5;
    J(0,2) = t3*t4;
    J(1,0) = 0.0;
    J(1,1) = t4;
    J(1,2) = -t5;
    J(2,0) = 1.0;
    J(2,1) = t3*t5*t6;
    J(2,2) = t3*t4*t6;
  }
Exemplo n.º 2
0
 /** \brief Computes the Jacobian for the update step of the filter.
  *
  *  @param F     - Jacobian for the update step of the filter.
  *  @param state - Filter state.
  *  @param meas  - Not used.
  *  @param dt    - Not used.
  */
 void jacInput(mtJacInput& F, const mtState& state, const mtMeas& meas, double dt = 0.0) const{
   F.setZero();
   F.template block<3,3>(mtInnovation::template getId<mtInnovation::_vel>(),mtState::template getId<mtState::_vel>()) = Eigen::Matrix3d::Identity();
 }