diff --git a/matlab/scripts/Inertial Nav EKF/Q_airdata.c b/matlab/scripts/Inertial Nav EKF/Q_airdata.c new file mode 100644 index 0000000000..690324a82d --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/Q_airdata.c @@ -0,0 +1,89 @@ +float t3 = ve-vwe; +float t4 = q0*q0; +float t5 = q1*q1; +float t6 = q2*q2; +float t7 = q3*q3; +float t8 = vd-vwd; +float t9 = q0*q1*2.0f; +float t10 = q2*q3*2.0f; +float t11 = vn-vwn; +float t13 = q0*q2*2.0f; +float t14 = q1*q3*2.0f; +float t18 = t4-t5-t6+t7; +float t19 = t8*t18; +float t20 = t9-t10; +float t21 = t3*t20; +float t22 = t13+t14; +float t23 = t11*t22; +float t2 = t19-t21+t23; +float t15 = q0*q3*2.0f; +float t16 = q1*q2*2.0f; +float t24 = t9+t10; +float t25 = t4-t5+t6-t7; +float t26 = t3*t25; +float t27 = t8*t24; +float t28 = t15-t16; +float t29 = t11*t28; +float t12 = t26+t27-t29; +float t30 = t13-t14; +float t31 = t4+t5-t6-t7; +float t32 = t11*t31; +float t33 = t8*t30; +float t34 = t15+t16; +float t35 = t3*t34; +float t17 = t32-t33+t35; +float t44 = t2*t18*2.0f; +float t45 = t12*t24*2.0f; +float t46 = t17*t30*2.0f; +float t36 = t44+t45-t46; +float t37 = t2*t2; +float t38 = t12*t12; +float t39 = t17*t17; +float t40 = t37+t38+t39; +float t41 = 1.0f/t40; +float t48 = t12*t25*2.0f; +float t49 = t2*t20*2.0f; +float t50 = t17*t34*2.0f; +float t42 = t48-t49+t50; +float t52 = t17*t31*2.0f; +float t53 = t2*t22*2.0f; +float t54 = t12*t28*2.0f; +float t43 = t52+t53-t54; +float t47 = t36*t36; +float t51 = t42*t42; +float t55 = t43*t43; +float t57 = 1.0f/(t17*t17); +float t58 = 1.0f/t17; +float t63 = t18*t58; +float t64 = t2*t30*t57; +float t56 = t63+t64; +float t66 = t22*t58; +float t67 = t2*t31*t57; +float t59 = t66-t67; +float t60 = t37*t57; +float t61 = t60+1.0f; +float t62 = 1.0f/(t61*t61); +float t65 = t56*t56; +float t68 = t59*t59; +float t70 = t20*t58; +float t71 = t2*t34*t57; +float t69 = t70+t71; +float t72 = t69*t69; +float t78 = t25*t58; +float t79 = t12*t34*t57; +float t73 = t78-t79; +float t81 = t28*t58; +float t82 = t12*t31*t57; +float t74 = t81+t82; +float t75 = t38*t57; +float t76 = t75+1.0f; +float t77 = 1.0f/(t76*t76); +float t80 = t73*t73; +float t83 = t74*t74; +float t85 = t24*t58; +float t86 = t12*t30*t57; +float t84 = t85+t86; +float t87 = t84*t84; +float tas_var = t41*t47*vd_var*0.25f+t41*t51*ve_var*0.25f+t41*t55*vn_var*0.25f+t41*t47*vwd_var*0.25f+t41*t51*vwe_var*0.25f+t41*t55*vwn_var*0.25f; +float aoa_var = t62*t65*vd_var+t62*t72*ve_var+t62*t68*vn_var+t62*t65*vwd_var+t62*t72*vwe_var+t62*t68*vwn_var; +float aos_var = t77*t87*vd_var+t77*t80*ve_var+t77*t83*vn_var+t77*t87*vwd_var+t77*t80*vwe_var+t77*t83*vwn_var; diff --git a/matlab/scripts/Inertial Nav EKF/derive_air_data_errors.m b/matlab/scripts/Inertial Nav EKF/derive_air_data_errors.m new file mode 100644 index 0000000000..1eae4a2f70 --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/derive_air_data_errors.m @@ -0,0 +1,97 @@ +% This script generates c code required to calculate the variance of the +% TAS, AoA and AoS estimates calculated from the vehicle quaternions, NED +% velocity and NED wind velocity. Uncertainty in the quaternions is +% ignored. Variance in vehicle velocity and wind velocity is accounted for. + +%% calculate TAS error terms +clear all; +reset(symengine); + +syms vn ve vd 'real' % navigation frame NED velocity (m/s) +syms vwn vwe vwd 'real' % navigation frame NED wind velocity (m/s) +syms vn_var ve_var vd_var 'real' % navigation frame NED velocity variances (m/s)^2 +syms vwn_var vwe_var vwd_var 'real' % navigation frame NED wind velocity variances (m/s)^2 +syms q0 q1 q2 q3 'real' % quaternions defining rotation from navigation NED frame to body XYZ frame + +quat = [q0;q1;q2;q3]; + +% rotation matrix from navigation to body frame +Tnb = transpose(Quat2Tbn(quat)); + +% crete velocity vectors +ground_velocity_truth = [vn;ve;vd]; +wind_velocity_truth = [vwn;vwe;vwd]; + +% calcuate wind relative velocity +rel_vel_ef = ground_velocity_truth - wind_velocity_truth; + +% rotate into body frame +rel_vel_bf = Tnb * rel_vel_ef; + +% calculate the true airspeed +TAS = sqrt(rel_vel_bf(1)^2 + rel_vel_bf(2)^2 + rel_vel_bf(3)^2); + +% derive the control(disturbance) influence matrix from velocity error to +% TAS error +G_TAS = jacobian(TAS, [ground_velocity_truth;wind_velocity_truth]); + +% derive the error matrix +TAS_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]); +Q_TAS = G_TAS*TAS_dist_matrix*transpose(G_TAS); + +% save as C code +ccode(Q_TAS,'file','Q_TAS.c'); + +save temp.mat; + +%% calculate AoA error equations +clear all; +reset(symengine); + +load temp.mat; + +AoA = atan(rel_vel_bf(3) / rel_vel_bf(1)); + +% derive the control(disturbance) influence matrix from velocity error to +% AoA error +G_AoA = jacobian(AoA, [ground_velocity_truth;wind_velocity_truth]); + +% derive the error matrix +AoA_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]); +Q_AoA = G_AoA*AoA_dist_matrix*transpose(G_AoA); + +% save as C code +ccode(Q_AoA,'file','Q_AoA.c'); + +save temp.mat; + +%% Calculate AoS error equations + +clear all; +reset(symengine); + +load temp.mat; + +AoS = atan(rel_vel_bf(2) / rel_vel_bf(1)); +% derive the control(disturbance) influence matrix from velocity error to +% AoS error +G_AoS = jacobian(AoS, [ground_velocity_truth;wind_velocity_truth]); + +% derive the error matrix +AoS_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]); +Q_AoS = G_AoS*AoS_dist_matrix*transpose(G_AoS); + +% save as C code +ccode(Q_AoS,'file','Q_AoS.c'); + +save temp.mat; + +%% convert them combined to take advantage of shared terms in the optimiser + +clear all; +reset(symengine); + +load temp.mat; + +% save as C code +ccode([Q_TAS;Q_AoA;Q_AoS],'file','Q_airdata.c');