592 float *hover_RPM_array,
float *skew,
float *elevator_angle,
FloatVect3 *V_gnd,
FloatVect3 *acc_filt,
float *V_pitot,
607 if (
eawp.health.crashes_n > floor(5.0f / dt)) {
608 eawp.health.healthy =
false;
617 eawp.inputs.accel(0) = acc->
x;
eawp.inputs.accel(1) = acc->
y;
eawp.inputs.accel(2) = acc->
z;
618 eawp.inputs.rates(0) = gyro->
p;
eawp.inputs.rates(1) = gyro->
q;
eawp.inputs.rates(2) = gyro->
r;
621 eawp.inputs.RPM_pusher = *pusher_RPM;
623 eawp.inputs.RPM_hover(0) = hover_RPM_array[0];
eawp.inputs.RPM_hover(1) = hover_RPM_array[1];
624 eawp.inputs.RPM_hover(2) = hover_RPM_array[2];
eawp.inputs.RPM_hover(3) = hover_RPM_array[3];
627 eawp.inputs.skew = *skew;
634 eawp.inputs.elevator_angle = *elevator_angle;
637 eawp.inputs.elevator_angle;
640 eawp.measurements.V_gnd(0) = V_gnd->
x;
eawp.measurements.V_gnd(1) = V_gnd->
y;
eawp.measurements.V_gnd(2) = V_gnd->
z;
641 eawp.measurements.accel_filt(0) = acc_filt->
x;
eawp.measurements.accel_filt(1) = acc_filt->
y;
642 eawp.measurements.accel_filt(2) = acc_filt->
z;
643 eawp.measurements.V_pitot = *V_pitot;
646 float V_a =
eawp.state.V_body.norm();
647 float u =
eawp.state.V_body(0);
648 float v =
eawp.state.V_body(1);
649 float w =
eawp.state.V_body(2);
650 float sign_u = u < 0.0f ? -1.0f : u > 0.0f ? 1.0f : 0.0f;
651 float sign_v = v < 0.0f ? -1.0f : v > 0.0f ? 1.0f : 0.0f;
653 float p =
eawp.inputs.rates(0);
654 float q =
eawp.inputs.rates(1);
655 float r =
eawp.inputs.rates(2);
657 float phi =
eawp.inputs.euler(0);
658 float theta =
eawp.inputs.euler(1);
659 float psi =
eawp.inputs.euler(2);
666 float cos_phi = cosf(phi);
667 float sin_phi = sinf(phi);
668 float cos_theta = cosf(theta);
669 float sin_theta = sinf(theta);
670 float cos_psi = cosf(psi);
671 float sin_psi = sinf(psi);
673 float cos_skew = cosf(
eawp.inputs.skew);
674 float sin_skew = sinf(
eawp.inputs.skew);
682 matrix::Matrix3f dcm;
683 dcm(0, 0) = cos_theta * cos_psi;
684 dcm(0, 1) = -cos_phi * sin_psi + sin_phi * sin_theta * cos_psi;
685 dcm(0, 2) = sin_phi * sin_psi + cos_phi * sin_theta * cos_psi;
687 dcm(1, 0) = cos_theta * sin_psi;
688 dcm(1, 1) = cos_phi * cos_psi + sin_phi * sin_theta * sin_psi;
689 dcm(1, 2) = -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi;
691 dcm(2, 0) = -sin_theta;
692 dcm(2, 1) = sin_phi * cos_theta;
693 dcm(2, 2) = cos_phi * cos_theta;
696 float aoa = atan2f(w, fabsf(u) < 1.E-5 ? 1
E-5 : u);
699 float tan_aoa = tanf(aoa);
702 float diff_alpha_u = 0.0f;
703 float diff_alpha_w = 0.0f;
704 if ((fabsf(u) > 1
E-5) & (fabsf(w) > 1
E-5)) {
705 diff_alpha_u = -w / (u * u + w * w);
706 diff_alpha_w = u / (u * u + w * w);
712 beta = asinf(v / V_a < -1.0f ? -1.0f : v / V_a > 1.0f ? 1.0f : v / V_a);
715 float hover_RPM_mean = (
eawp.inputs.RPM_hover(0) +
eawp.inputs.RPM_hover(1) +
eawp.inputs.RPM_hover(
716 2) +
eawp.inputs.RPM_hover(3)) / 4.0f;
818 matrix::Vector3f state_dev = -
eawp.inputs.rates.cross(
eawp.state.V_body) + dcm.transpose() *
gravity +
823 for (
int i = 0; i < 3; i++) {
824 if (std::isnan(state_dev(i))) {
831 eawp.health.healthy =
false;
832 eawp.health.crashes_n += 1;
834 eawp.state.V_body += state_dev * dt;
850 1)) + v * v *
eawp.Q(5, 5) + w * w *
eawp.Q(4, 4);
858 0)) + u * u *
eawp.Q(5, 5) + w * w *
eawp.Q(3, 3);
866 0)) + u * u *
eawp.Q(4, 4) + v * v *
eawp.Q(3, 3);
948 a_x = (
eawp.forces.fuselage(0) +
949 eawp.forces.wing(0) +
950 eawp.forces.elevator(0) +
951 eawp.forces.hover(0) +
952 eawp.forces.pusher(0) +
986 a_z = (
eawp.forces.fuselage(2) +
987 eawp.forces.elevator(2) +
988 eawp.forces.wing(2) +
991 a_z =
eawp.measurements.accel_filt(2);
994 matrix::Vector3f accel_est = {a_x, a_y, a_z};
997 eawp.innovations.V_gnd =
eawp.measurements.V_gnd - (dcm *
eawp.state.V_body +
999 eawp.innovations.accel_filt =
eawp.measurements.accel_filt - accel_est;
1000 eawp.innovations.V_pitot =
eawp.measurements.V_pitot -
eawp.state.V_body(0);
1029 matrix::Matrix<float, EKF_AW_R_SIZE, EKF_AW_COV_SIZE> G;
1033 G(0, 0) = cos_psi * cos_theta;
1034 G(0, 1) = cos_psi * sin_phi * sin_theta - cos_phi * sin_psi;
1035 G(0, 2) = sin_phi * sin_psi + cos_phi * cos_psi * sin_theta;
1037 G(1, 0) = cos_theta * sin_psi;
1038 G(1, 1) = cos_phi * cos_psi + sin_phi * sin_psi * sin_theta;
1039 G(1, 2) = cos_phi * sin_psi * sin_theta - cos_psi * sin_phi;
1041 G(2, 0) = -sin_theta;
1042 G(2, 1) = cos_theta * sin_phi;
1043 G(2, 2) = cos_phi * cos_theta;
1066 * hover_RPM_mean) / sqrtf(fabsf(fabsf(u) < 1
E-5 ? 1
E-5 : u)) +
ekf_aw_params.k_fx_hover[2] * sign_u) /
1069#if EKF_AW_WING_INSTALLED
1074 float temp_3 = sin_skew * sin_skew +
ekf_aw_params.k_fx_wing[3];
1082 float temp_4 =
ekf_aw_params.k_fx_elev[2] *
eawp.inputs.elevator_angle *
eawp.inputs.elevator_angle +
1097 G(3, 0) += (2 *
eawp.state.offset(0) * sign_u * u) /
ekf_aw_params.vehicle_mass;
1102 float protected_V_a = fabsf(V_a) < 1
E-5 ? 1
E-5 : V_a;
1103 float cos_beta = fabsf(cosf(beta)) < 1
E-5 ? 1
E-5 : cosf(beta);
1111#if EKF_AW_WING_INSTALLED
1131#if EKF_AW_WING_INSTALLED
1140 ekf_aw_params.k_fz_wing[2] * aoa * diff_alpha_w) * V_a * V_a + 2 * w * (sin_skew * sin_skew +
1166 matrix::SquareMatrix<float, EKF_AW_R_SIZE> S = G *
eawp.P * G.transpose() +
eawp.R;
1174 matrix::Matrix<float, EKF_AW_COV_SIZE, EKF_AW_R_SIZE>
K =
eawp.P * G.transpose() * S.I();
1185 if (std::isnan(
K(i, j))) {
1193 eawp.health.healthy =
false;
1194 eawp.health.crashes_n += 1;
1196 eawp.health.healthy =
true;
1199 matrix::Matrix<float, 3, 3> K_slice_3;
1200 matrix::Matrix<float, 3, 1> K_slice_1;
1203 K_slice_3 =
K.slice<3, 3>(0, 0);
eawp.state.V_body += K_slice_3 *
eawp.innovations.V_gnd;
1204 K_slice_3 =
K.slice<3, 3>(3, 0);
eawp.state.wind += K_slice_3 *
eawp.innovations.V_gnd;
1206 K_slice_3 =
K.slice<3, 3>(6, 0);
eawp.state.offset += K_slice_3 *
eawp.innovations.V_gnd;
1210 K_slice_3 =
K.slice<3, 3>(0, 3);
eawp.state.V_body += K_slice_3 *
eawp.innovations.accel_filt;
1211 K_slice_3 =
K.slice<3, 3>(3, 3);
eawp.state.wind += K_slice_3 *
eawp.innovations.accel_filt;
1213 K_slice_3 =
K.slice<3, 3>(6, 3);
eawp.state.offset += K_slice_3 *
eawp.innovations.accel_filt;
1218 K_slice_1 =
K.slice<3, 1>(0, 6);
eawp.state.V_body += K_slice_1 *
eawp.innovations.V_pitot;
1219 K_slice_1 =
K.slice<3, 1>(3, 6);
eawp.state.wind += K_slice_1 *
eawp.innovations.V_pitot;
1221 K_slice_1 =
K.slice<3, 1>(6, 6);
eawp.state.offset += K_slice_1 *
eawp.innovations.V_pitot;
1230 matrix::SquareMatrix<float, EKF_AW_COV_SIZE> eye;
1239 eawp.forces.fuselage(0) = duration_1 * 1.0f;
1240 eawp.forces.fuselage(1) = duration_2 * 1.0f;
1241 eawp.forces.fuselage(2) = duration_3 * 1.0f;
1243 eawp.forces.wing(0) = duration_4 * 1.0f;
1244 eawp.forces.wing(1) = duration_5 * 1.0f;
1245 eawp.forces.wing(2) = duration_6 * 1.0f;
1247 eawp.forces.elevator(0) = duration_7 * 1.0f;
1248 eawp.forces.elevator(1) = duration_8 * 1.0f;
1249 eawp.forces.elevator(2) = duration_9 * 1.0f;
1251 eawp.forces.hover(0) = duration_10 * 1.0f;
1252 eawp.forces.hover(1) = duration_11 * 1.0f;