Possible issue with GEstG in imu_calc

In imu_calc an updated prediction for the gravity vector in the quadrotor body frame is calculated based on small angle approximation

state.GEstG.axis[2] = state.GEstG.axis[2] - (gyro_delta_angle[0]) * state.GEstG.axis[0];
state.GEstG.axis[0] = (gyro_delta_angle[0]) * state.GEstG.axis[2] + state.GEstG.axis[0];

state.GEstG.axis[1] = state.GEstG.axis[1] + (gyro_delta_angle[1]) * state.GEstG.axis[2];
state.GEstG.axis[2] = -(gyro_delta_angle[1]) * state.GEstG.axis[1] + state.GEstG.axis[2];

state.GEstG.axis[0] = state.GEstG.axis[0] - (gyro_delta_angle[2]) * state.GEstG.axis[1];
state.GEstG.axis[1] = (gyro_delta_angle[2]) * state.GEstG.axis[0] + state.GEstG.axis[1];

However, I was not able to reconcile the implementation with the approach. I'm therefore posting this for some discussion. Additionally I believe GEstG should be normalized after the prediction since the small angle approximation to the rotation matrix does not preserve lengths. There's no point in keeping the length anything else but unit length since the magnitude of the vector does not carry information about orientation.

There are several ways to update GEstG and I've compared numerous ways with the implementation and none of them coincided with the implementation although they were consistent among themselves.

Formally the update can be done in the following way

dot_R = R*S(w)      //S() denotes skew-symmetric matrix of w, where w is the angular velocity of the rigid-body
dR = R+ dt*dot_R  //Reorthogonalize after forward Euler, alternatively use variational integrators
GEstG = transpose(dR)*GEstG

dR is the rotation matrix that rotates vectors from the updated body frame to the old body frame. Since we are doing this incrementally the previous attitude R coincides with the body frame and is therefore the identity matrix I.

Alternatively we can take advantage of the fact that GEstG is a vector of constant length and it's derivative can be expressed as

dot_GEstG = cross(w,GEstG)     //cross(a,b) denotes the cross product between vectors a and b
GEstG = GEstG - dt*dot_GEstG //Normalize result since forward Euler does not preserve the length.

We can validate these approaches with a simple test and compare it with the above implementation. Let the gyro measurement be w = [1,0,0] with a sampling time of dt = 0.1. Assume the initial orientation aligns with the inertial one i.e. R = I. In this case the body frame is rotating around the x-axis which means the gravity vector will rotate in the yz-plane so we would expect the gravity vector to go from only being along the negative z-axis to a vector that has both a z-axis and y-axis component. Using the above implementation we get the result

GEstG = [-0.0995 0 -0.995]'    //Current implementation
GEstG = [0 -0.0998 -0.995]'    //Axis angle approach
GEstG = [0 -0.0999 -0.0995]'  //No small angle approximation
GEstG = [0 -0.0995 -0.995]'    //Small angle approximation

As you can see the current implementation gives an updated gravity vector that has an x-component instead of a y-component, which is not correct.

The axis angle is based on the midpoint method for integrating differential equations. The accuracy is far greater than the small angle approximation and the computational cost is competitive. The approach is as follows

GEstG_mid = GEstG - dt/2*cross(w, GEstG)
GEstG_mid /= magnitude(GEstG_mid)
GEstG = GEstG - dt*cross(w, GEstG_mid)
GEstG /= magnitude(GEstG)

Alternatively if a second order method is not worth it you can still do a first order one which still have the same accuracy as the small angle approximation and similar computational cost

GEstG = GEstG - dt*cross(w, GEstG)
GEstG /= magnitude(GEstG)

BossHobby / QUICKSILVER

Possible issue with GEstG in imu_calc #69