void quat_vector3_rotate (geometry_msgs::Quaternion q, geometry_msgs::Vector3 v, geometry_msgs::Vector3 *res) { geometry_msgs::Quaternion vq, q_inv, qa, qb; vq.w = 0.0; vq.x = v.x; vq.y = v.y; vq.z = v.z; quat_inverse (q, & q_inv); quat_product (q, vq, & qa); quat_product (qa, q_inv, & qb); res -> x = qb.x; res -> y = qb.y; res -> z = qb.z; }
//Propagation of the states void ForwardEuler::propagation() { geometry_msgs::Vector3 tempVect; // Propagate the NED coordinates from earth velocity parentObj->states.pose.position.x = parentObj->states.pose.position.x + parentObj->kinematics.posDot.x * parentObj->dt; parentObj->states.pose.position.y = parentObj->states.pose.position.y + parentObj->kinematics.posDot.y * parentObj->dt; parentObj->states.pose.position.z = parentObj->states.pose.position.z + parentObj->kinematics.posDot.z * parentObj->dt; tempVect.x = parentObj->states.pose.position.x; tempVect.y = parentObj->states.pose.position.y; tempVect.z = parentObj->states.pose.position.z; if (isnan(tempVect)) {ROS_FATAL("NaN member in position vector"); ros::shutdown();} // Propagate orientation quaternion from angular derivatives quaternion geometry_msgs::Quaternion quat = parentObj->states.pose.orientation; quat_product(quat,parentObj->kinematics.quatDot,&(parentObj->states.pose.orientation)); quat_normalize(&(parentObj->states.pose.orientation)); if (isnan(parentObj->states.pose.orientation)) {ROS_FATAL("NaN member in orientation quaternion"); ros::shutdown();} // Propagate body velocity from body velocity derivatives tempVect = parentObj->dt*parentObj->kinematics.speedDot; parentObj->states.velocity.linear = parentObj->states.velocity.linear + tempVect; if (isnan(parentObj->states.velocity.linear)) {ROS_FATAL("NaN member in linear velocity vector"); ros::shutdown();} // Propagates angular velocity from angular derivatives tempVect = parentObj->dt*parentObj->kinematics.rateDot; parentObj->states.velocity.angular = parentObj->states.velocity.angular + tempVect; if (isnan(parentObj->states.velocity.angular)) {ROS_FATAL("NaN member in angular velocity vector"); ros::shutdown();} // Set linear acceleration from the speed derivatives parentObj->states.acceleration.linear = parentObj->kinematics.speedDot; // Set angular acceleration from the angular rate derivatives parentObj->states.acceleration.angular = parentObj->kinematics.rateDot; //Update Geoid stuff using the NED coordinates parentObj->states.geoid.latitude += 180.0/M_PI*asin(parentObj->kinematics.posDot.x * parentObj->dt / WGS84_RM(parentObj->states.geoid.latitude)); parentObj->states.geoid.longitude += 180.0/M_PI*asin(parentObj->kinematics.posDot.y * parentObj->dt / WGS84_RN(parentObj->states.geoid.longitude)); parentObj->states.geoid.altitude += -parentObj->kinematics.posDot.z * parentObj->dt; parentObj->states.geoid.velocity.x = parentObj->kinematics.posDot.x; parentObj->states.geoid.velocity.y = parentObj->kinematics.posDot.y; parentObj->states.geoid.velocity.z = -parentObj->kinematics.posDot.z; // Upwards velocity }