fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 22:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #134 from PX4/pr-ekf2Alignment

Browse Source 
EKF: Improved tilt alignment control
This commit is contained in:
Paul Riseborough 2016-05-18 12:34:22 +10:00
commit 50ba26a434
8 changed files with 425 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
EKF/common.h
View File

@ -197,6 +197,11 @@ struct parameters {
float terrain_p_noise; // process noise for terrain offset (m/sec)
float terrain_gradient; // gradient of terrain used to estimate process noise due to changing position (m/m)
// initialisation errors
float switch_on_gyro_bias; // 1-sigma gyro bias uncertainty at switch on (rad/sec)
float switch_on_accel_bias; // 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
float initial_tilt_err; // 1-sigma tilt error after initial alignment using gravity vector (rad)
// position and velocity fusion
float gps_vel_noise; // observation noise for gps velocity fusion (m/sec)
float gps_pos_noise; // observation noise for gps position fusion (m)
@ -288,6 +293,11 @@ struct parameters {
terrain_p_noise = 5.0f;
terrain_gradient = 0.5f;
// initialisation errors
switch_on_gyro_bias = 0.1f;
switch_on_accel_bias = 0.2f;
initial_tilt_err = 0.1f;
// position and velocity fusion
gps_vel_noise = 5.0e-1f;
gps_pos_noise = 0.5f;

15
EKF/control.cpp
View File

@ -49,11 +49,16 @@ void Ekf::controlFusionModes()
// Get the magnetic declination
calcMagDeclination();
// Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states
float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3];
if (total_angle_variance < 0.002f && !_control_status.flags.tilt_align) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances
Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced sufficiently, initialise the yaw and magnetic field states
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < 0.002f) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
}
// control use of various external souces for positon and velocity aiding

15
EKF/covariance.cpp
View File

@ -55,11 +55,12 @@ void Ekf::initialiseCovariance()
// calculate average prediction time step in sec
float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS;
// quaternion error
P[0][0] = 0.01f;
P[1][1] = 0.01f;
P[2][2] = 0.01f;
P[3][3] = 0.01f;
// define the initial angle uncertainty as variances for a rotation vector
Vector3f rot_vec_var;
rot_vec_var(2) = rot_vec_var(1) = rot_vec_var(0) = sq(_params.initial_tilt_err);
// update the quaternion state covariances
initialiseQuatCovariances(rot_vec_var);
// velocity
P[4][4] = sq(fmaxf(_params.gps_vel_noise, 0.01f));
@ -80,12 +81,12 @@ void Ekf::initialiseCovariance()
}
// gyro bias
P[10][10] = sq(0.1f * dt);
P[10][10] = sq(_params.switch_on_gyro_bias * dt);
P[11][11] = P[10][10];
P[12][12] = P[10][10];
// accel bias
P[13][13] = sq(0.2f * dt);
P[13][13] = sq(_params.switch_on_accel_bias * dt);
P[14][14] = P[13][13];
P[15][15] = P[13][13];

6
EKF/ekf.h
View File

@ -363,4 +363,10 @@ private:
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar();
// rotate quaternion covariances into variances for an equivalent rotation vector
Vector3f calcRotVecVariances();
// initialise the quaternion covariances using rotation vector variances
void initialiseQuatCovariances(Vector3f &rot_vec_var);
};

271
EKF/ekf_helper.cpp
View File

@ -302,35 +302,119 @@ void Ekf::alignOutputFilter()
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
// If we don't a tilt estimate then we cannot initialise the yaw
if (!_control_status.flags.tilt_align) {
return false;
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// calculate the initial quaternion
// determine if a 321 or 312 Euler sequence is best
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
// use a 321 sequence
// rotate the magnetometer measurement into earth frame
matrix::Euler<float> euler321(_state.quat_nominal);
// Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
euler321(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler321);
// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
// convert the observed quaternion to a rotation matrix
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler321(2) = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
}
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(euler321);
} else {
// use a 312 sequence
// Calculate the 312 sequence euler angles that rotate from earth to body frame
// See http://www.atacolorado.com/eulersequences.doc
Vector3f euler312;
euler312(0) = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw)
euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
euler312(2) = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch)
// Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
euler312(0) = 0.0f;
// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
float c2 = cosf(euler312(2));
float s2 = sinf(euler312(2));
float s1 = sinf(euler312(1));
float c1 = cosf(euler312(1));
float s0 = sinf(euler312(0));
float c0 = cosf(euler312(0));
matrix::Dcm<float> R_to_earth;
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
R_to_earth(0, 1) = -c1 * s0;
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
R_to_earth(2, 0) = -s2 * c1;
R_to_earth(2, 1) = s1;
// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
// convert the observed quaternion to a rotation matrix
matrix::Dcm<float> R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
euler312(0) = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
} else {
// there is no yaw observation
return false;
}
// re-calculate the rotation matrix using the updated yaw angle
s0 = sinf(euler312(0));
c0 = cosf(euler312(0));
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
R_to_earth(1, 1) = c0 * c1;
R_to_earth(2, 2) = c2 * c1;
R_to_earth(0, 1) = -c1 * s0;
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
R_to_earth(2, 0) = -s2 * c1;
R_to_earth(2, 1) = s1;
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(R_to_earth);
}
// get the roll, pitch, yaw estimates and set the yaw to zero
matrix::Quaternion<float> q(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
_state.quat_nominal(3));
matrix::Euler<float> euler_init(q);
euler_init(2) = 0.0f;
// rotate the magnetometer measurements into earth axes
matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
euler_init(2) = _mag_declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
// calculate initial quaternion states for the ekf
// we don't change the output attitude to avoid jumps
_state.quat_nominal = Quaternion(euler_init);
// reset the quaternion variances because the yaw angle could have changed by a significant amount
// by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise
// will grow them again.
zeroRows(P, 0, 3);
zeroCols(P, 0, 3);
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// calculate initial earth magnetic field states
matrix::Dcm<float> R_to_earth(euler_init);
_state.mag_I = R_to_earth * mag_init;
_state.mag_I = _R_to_earth * mag_init;
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
zeroRows(P, 16, 21);
@ -654,3 +738,150 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quaternion quat)
return dcm;
}
// calculate the variances for the rotation vector equivalent
Vector3f Ekf::calcRotVecVariances()
{
Vector3f rot_var_vec;
float q0,q1,q2,q3;
if (_state.quat_nominal(0) >= 0.0f) {
q0 = _state.quat_nominal(0);
q1 = _state.quat_nominal(1);
q2 = _state.quat_nominal(2);
q3 = _state.quat_nominal(3);
} else {
q0 = -_state.quat_nominal(0);
q1 = -_state.quat_nominal(1);
q2 = -_state.quat_nominal(2);
q3 = -_state.quat_nominal(3);
}
float t2 = q0*q0;
float t3 = acos(q0);
float t4 = -t2+1.0f;
float t5 = t2-1.0f;
float t6 = 1.0f/t5;
float t7 = q1*t6*2.0f;
float t8 = 1.0f/powf(t4,1.5f);
float t9 = q0*q1*t3*t8*2.0f;
float t10 = t7+t9;
float t11 = 1.0f/sqrtf(t4);
float t12 = q2*t6*2.0f;
float t13 = q0*q2*t3*t8*2.0f;
float t14 = t12+t13;
float t15 = q3*t6*2.0f;
float t16 = q0*q3*t3*t8*2.0f;
float t17 = t15+t16;
rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f;
rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f;
rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f;
return rot_var_vec;
}
// initialise the quaternion covariances using rotation vector variances
void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
{
// calculate an equivalent rotation vector from the quaternion
float q0,q1,q2,q3;
if (_state.quat_nominal(0) >= 0.0f) {
q0 = _state.quat_nominal(0);
q1 = _state.quat_nominal(1);
q2 = _state.quat_nominal(2);
q3 = _state.quat_nominal(3);
} else {
q0 = -_state.quat_nominal(0);
q1 = -_state.quat_nominal(1);
q2 = -_state.quat_nominal(2);
q3 = -_state.quat_nominal(3);
}
float delta = 2.0f*acosf(q0);
float scaler = (delta/sinf(delta*0.5f));
float rotX = scaler*q1;
float rotY = scaler*q2;
float rotZ = scaler*q3;
// autocode generated using matlab symbolic toolbox
float t2 = rotX*rotX;
float t4 = rotY*rotY;
float t5 = rotZ*rotZ;
float t6 = t2+t4+t5;
if (t6 > 1e-9f) {
float t7 = sqrtf(t6);
float t8 = t7*0.5f;
float t3 = sinf(t8);
float t9 = t3*t3;
float t10 = 1.0f/t6;
float t11 = 1.0f/sqrtf(t6);
float t12 = cosf(t8);
float t13 = 1.0f/powf(t6,1.5f);
float t14 = t3*t11;
float t15 = rotX*rotY*t3*t13;
float t16 = rotX*rotZ*t3*t13;
float t17 = rotY*rotZ*t3*t13;
float t18 = t2*t10*t12*0.5f;
float t27 = t2*t3*t13;
float t19 = t14+t18-t27;
float t23 = rotX*rotY*t10*t12*0.5f;
float t28 = t15-t23;
float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f;
float t25 = rotX*rotZ*t10*t12*0.5f;
float t31 = t16-t25;
float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f;
float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f;
float t24 = t15-t23;
float t26 = t16-t25;
float t29 = t4*t10*t12*0.5f;
float t34 = t3*t4*t13;
float t30 = t14+t29-t34;
float t32 = t5*t10*t12*0.5f;
float t40 = t3*t5*t13;
float t33 = t14+t32-t40;
float t36 = rotY*rotZ*t10*t12*0.5f;
float t39 = t17-t36;
float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f;
float t37 = t15-t23;
float t38 = t17-t36;
float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25);
float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39;
float t43 = t16-t25;
float t44 = t17-t36;
// auto-code generated using matlab symbolic toolbox
P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
P[0][1] = t22;
P[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;
P[0][3] = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f;
P[1][0] = t22;
P[1][1] = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26);
P[1][2] = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
P[1][3] = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
P[2][0] = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f;
P[2][1] = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
P[2][2] = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38);
P[2][3] = t42;
P[3][0] = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f;
P[3][1] = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
P[3][2] = t42;
P[3][3] = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44);
} else {
// the equations are badly conditioned so use a small angle approximation
P[0][0] = 0.0f;
P[0][1] = 0.0f;
P[0][2] = 0.0f;
P[0][3] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.25f*rot_vec_var(0);
P[1][2] = 0.0f;
P[1][3] = 0.0f;
P[2][0] = 0.0f;
P[2][1] = 0.0f;
P[2][2] = 0.25f*rot_vec_var(1);
P[2][3] = 0.0f;
P[3][0] = 0.0f;
P[3][1] = 0.0f;
P[3][2] = 0.0f;
P[3][3] = 0.25f*rot_vec_var(2);
}
}

62
matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m Normal file
View File

@ -0,0 +1,62 @@
%% calculate the rotation vector variances from an equivalent quaternion
% inputs are the quaternion orientation and the 4x4 covariance matrix for the quaternions
% output is a vector of variances for the rotation vector that is equivalent to the quaternion
clear all;
reset(symengine);
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
% define quaternion rotation
quat = [q0;q1;q2;q3];
% convert to a rotation vector
delta = 2*acos(q0);
rotVec = (delta/sin(delta/2))*[q1;q2;q3];
% calculate transfer matrix from quaternion to rotation vector
G = jacobian(rotVec, quat);
% define a symbolic covariance matrix using strings to represent
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:4
for colIndex = 1:4
eval(['syms P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1), '_r_ real']);
eval(['quatCovMat(',num2str(rowIndex),',',num2str(colIndex), ') = P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1),'_r_;']);
end
end
% rotate the covariance from quaternion to rotation vector
rotCovMat = G*quatCovMat*transpose(G);
% take the variances
rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)];
% convert to c-code
ccode(rotVarVec,'file','rotVarVec.c');
%% calculate the quaternion variances from an equivalent rotation vector
% define a rotation vector
syms rotX rotY rotZ real;
rotVec = [rotX;rotY;rotZ];
% convert to a quaternion
vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
quat = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
% calculate transfer matrix from rotation vector to quaternion
G = jacobian(quat, rotVec);
% define the rotation vector variances
syms rotVarX rotVarY rotVarZ real;
% define the rotation vector covariance matrix
rotCovMat = diag([rotVarX;rotVarY;rotVarZ]);
% rotate the covariance matrix into quaternion coordinates
quatCovMat = G*rotCovMat*transpose(G);
% convert to c-code
ccode(quatCovMat,'file','quatCovMat.c');

59
matlab/scripts/Inertial Nav EKF/quatCovMat.c Normal file
View File

@ -0,0 +1,59 @@
float t2 = rotX*rotX;
float t4 = rotY*rotY;
float t5 = rotZ*rotZ;
float t6 = t2+t4+t5;
float t7 = sqrtf(t6);
float t8 = t7*0.5f;
float t3 = sinf(t8);
float t9 = t3*t3;
float t10 = 1.0f/t6;
float t11 = 1.0f/sqrtf(t6);
float t12 = cosf(t8);
float t13 = 1.0f/powf(t6,1.5f);
float t14 = t3*t11;
float t15 = rotX*rotY*t3*t13;
float t16 = rotX*rotZ*t3*t13;
float t17 = rotY*rotZ*t3*t13;
float t18 = t2*t10*t12*0.5f;
float t27 = t2*t3*t13;
float t19 = t14+t18-t27;
float t23 = rotX*rotY*t10*t12*0.5f;
float t28 = t15-t23;
float t20 = rotY*rotVarY*t3*t11*t28*0.5f;
float t25 = rotX*rotZ*t10*t12*0.5f;
float t31 = t16-t25;
float t21 = rotZ*rotVarZ*t3*t11*t31*0.5f;
float t22 = t20+t21-rotX*rotVarX*t3*t11*t19*0.5f;
float t24 = t15-t23;
float t26 = t16-t25;
float t29 = t4*t10*t12*0.5f;
float t34 = t3*t4*t13;
float t30 = t14+t29-t34;
float t32 = t5*t10*t12*0.5f;
float t40 = t3*t5*t13;
float t33 = t14+t32-t40;
float t36 = rotY*rotZ*t10*t12*0.5f;
float t39 = t17-t36;
float t35 = rotZ*rotVarZ*t3*t11*t39*0.5f;
float t37 = t15-t23;
float t38 = t17-t36;
float t41 = rotVarX*(t15-t23)*(t16-t25);
float t42 = t41-rotVarY*t30*t39-rotVarZ*t33*t39;
float t43 = t16-t25;
float t44 = t17-t36;
P[0][0] = rotVarX*t2*t9*t10*0.25f+rotVarY*t4*t9*t10*0.25f+rotVarZ*t5*t9*t10*0.25f;
P[0][1] = t22;
P[0][2] = t35+rotX*rotVarX*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarY*t3*t11*t30*0.5f;
P[0][3] = rotX*rotVarX*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarY*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarZ*t3*t11*t33*0.5f;
P[1][0] = t22;
P[1][1] = rotVarX*(t19*t19)+rotVarY*(t24*t24)+rotVarZ*(t26*t26);
P[1][2] = rotVarZ*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t28-rotVarY*t28*t30;
P[1][3] = rotVarY*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t31-rotVarZ*t31*t33;
P[2][0] = t35-rotY*rotVarY*t3*t11*t30*0.5f+rotX*rotVarX*t3*t11*(t15-t23)*0.5f;
P[2][1] = rotVarZ*(t16-t25)*(t17-t36)-rotVarX*t19*t28-rotVarY*t28*t30;
P[2][2] = rotVarY*(t30*t30)+rotVarX*(t37*t37)+rotVarZ*(t38*t38);
P[2][3] = t42;
P[3][0] = rotZ*rotVarZ*t3*t11*t33*(-1.0f/2.0f)+rotX*rotVarX*t3*t11*(t16-t25)*0.5f+rotY*rotVarY*t3*t11*(t17-t36)*0.5f;
P[3][1] = rotVarY*(t15-t23)*(t17-t36)-rotVarX*t19*t31-rotVarZ*t31*t33;
P[3][2] = t42;
P[3][3] = rotVarZ*(t33*t33)+rotVarX*(t43*t43)+rotVarY*(t44*t44);

19
matlab/scripts/Inertial Nav EKF/rotVarVec.c Normal file
View File

@ -0,0 +1,19 @@
t2 = q0*q0;
t3 = acos(q0);
t4 = -t2+1.0;
t5 = t2-1.0;
t6 = 1.0/t5;
t7 = q1*t6*2.0;
t8 = 1.0/pow(t4,3.0/2.0);
t9 = q0*q1*t3*t8*2.0;
t10 = t7+t9;
t11 = 1.0/sqrt(t4);
t12 = q2*t6*2.0;
t13 = q0*q2*t3*t8*2.0;
t14 = t12+t13;
t15 = q3*t6*2.0;
t16 = q0*q3*t3*t8*2.0;
t17 = t15+t16;
A0[0][0] = t10*(P_l_0_c_0_r_*t10+P_l_1_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_1_r_*t10+P_l_1_c_1_r_*t3*t11*2.0)*2.0;
A0[1][0] = t14*(P_l_0_c_0_r_*t14+P_l_2_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_2_r_*t14+P_l_2_c_2_r_*t3*t11*2.0)*2.0;
A0[2][0] = t17*(P_l_0_c_0_r_*t17+P_l_3_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_3_r_*t17+P_l_3_c_3_r_*t3*t11*2.0)*2.0;