void Ekf::calculateOutputStates() { imuSample imu_new = _imu_sample_new; Vector3f delta_angle; // Note: We do no not need to consider any bias or scale correction here // since the base class has already corrected the imu sample delta_angle(0) = imu_new.delta_ang(0); delta_angle(1) = imu_new.delta_ang(1); delta_angle(2) = imu_new.delta_ang(2); Vector3f delta_vel = imu_new.delta_vel; delta_angle += _delta_angle_corr; Quaternion dq; dq.from_axis_angle(delta_angle); _output_new.time_us = imu_new.time_us; _output_new.quat_nominal = dq * _output_new.quat_nominal; _output_new.quat_nominal.normalize(); matrix::Dcm<float> R_to_earth(_output_new.quat_nominal); Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr; delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt; Vector3f vel_last = _output_new.vel; _output_new.vel += delta_vel_NED; _output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt; if (_imu_updated) { _output_buffer.push(_output_new); _imu_updated = false; } _output_sample_delayed = _output_buffer.get_oldest(); Quaternion quat_inv = _state.quat_nominal.inversed(); Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv; q_error.normalize(); Vector3f delta_ang_error; float scalar; if (q_error(0) >= 0.0f) { scalar = -2.0f; } else { scalar = 2.0f; } delta_ang_error(0) = scalar * q_error(1); delta_ang_error(1) = scalar * q_error(2); delta_ang_error(2) = scalar * q_error(3); _delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt; _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt; _vel_corr = (_state.pos - _output_sample_delayed.pos); }
/** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) * Output: '_rates_sp' vector, '_thrust_sp' */ void MulticopterAttitudeControl::control_attitude(float dt) { vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; /* construct attitude setpoint rotation matrix */ math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]); math::Matrix<3, 3> R_sp = q_sp.to_dcm(); /* get current rotation matrix from control state quaternions */ math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); math::Matrix<3, 3> R = q_att.to_dcm(); /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ float e_R_z_sin = e_R.length(); float e_R_z_cos = R_z * R_sp_z; /* calculate weight for yaw control */ float yaw_w = R_sp(2, 2) * R_sp(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix<3, 3> R_rp; if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; e_R = e_R_z_axis * e_R_z_angle; /* cross product matrix for e_R_axis */ math::Matrix<3, 3> e_R_cp; e_R_cp.zero(); e_R_cp(0, 1) = -e_R_z_axis(2); e_R_cp(0, 2) = e_R_z_axis(1); e_R_cp(1, 0) = e_R_z_axis(2); e_R_cp(1, 2) = -e_R_z_axis(0); e_R_cp(2, 0) = -e_R_z_axis(1); e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ R_rp = R; } /* R_rp and R_sp has the same Z axis, calculate yaw error */ math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; if (e_R_z_cos < 0.0f) { /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q_error; q_error.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag() * 2.0f : -q_error.imag() * 2.0f; /* use fusion of Z axis based rotation and direct rotation */ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; } /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); /* limit rates */ for (int i = 0; i < 3; i++) { if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && !_v_control_mode.flag_control_manual_enabled) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i)); } else { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; /* weather-vane mode, dampen yaw rate */ if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) { float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); // prevent integrator winding up in weathervane mode _rates_int(2) = 0.0f; } }