matlab: add derivation for air data error equations

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;

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');