External Pose from 90' to -90' passing from 180 get crazy

[Marcox2111]

Hi, i'm doing a project that involves the use of qualisys to feed the position but I found a problem that are days that I'm trying to solve.
Feeding with extpose, using quaternions works pretty good except for a case. When my device goes from 90 degree to 180 degree and from -180 to -90 degree the state estimation doesn't work anymore.

I have a picture of the output of qualysis and the output of the state estimation. (don't mind if one is in rad and one is in deg with opposite sign)

[krichardsson]

Hi!

I have also seen instabilities around 90 degrees, I'm not sure why though. I got the feeling that the estimator is more sensitive to measurement noise in the quaternion data close to 90 degrees and I ended up increasing the standard deviation when pushing samples to it.

You can set the standard deviation for extpos quaternion with he parameter locSrv.extQuatStdDev

[Marcox2111]

Hi!

I have also seen instabilities around 90 degrees, I'm not sure why though. I got the feeling that the estimator is more sensitive to measurement noise in the quaternion data close to 90 degrees and I ended up increasing the standard deviation when pushing samples to it.

You can set the standard deviation for extpose quaternion with the parameter locSrv.extQuatStdDev

Thanks for the reply
In the meantime, I did some workaround in the code to understand what is happening. I was already playing with that parameter but due to my lack of knowledge about the kalman filter I just thought it was necessary to decrease it to increase the "trust" of this sensor on the others, by reducing the stddev by a lot, didn't work. I tried several things, like removing the flow deck for instance and it seams that has no effect on this problem. I started to think about some sort of gimbal lock that at first glance seems useless because this function works with quaternions but by looking at code in the firmware these quaternions will be converted into Euler angles, but after other tests, i started to exclude this phenomenon.

As a first solution (bypass of the problem), i disabled in "estimator_kalman.c" all the sensors but KalmanUpdateWithPose and it worked as supposed (apart from some strange noise on the Z axis but is something that is not very annoying), starting from that i started to include sensors one at time but i still didn't completed the research to find which sensor or which bug create this situation. I will update you soon, in the meantime i will study better how the stddev works for the kalman filter and if there is a way to let it work without doing strange stuff.

Thank you

[krichardsson]

Another possibility is to only push in the position (not full pose), the drawback is that this requires that you start the Crazyflie facing the positive X direction (or tell the kalman estimator what the current yaw is just before takeoff). Roll, pitch and yaw will be estimated from the IMU and motion of the drone and works pretty well also.

If you want to try this, you can find a function in the python lib, or message in the Crazyflie (depending how your system works) that only takes position but no attitude.

[Marcox2111]

Another possibility is to only push in the position (not full pose), the drawback is that this requires that you start the Crazyflie facing the positive X direction (or tell the kalman estimator what the current yaw is just before takeoff). Roll, pitch and yaw will be estimated from the IMU and motion of the drone and works pretty well also.

If you want to try this, you can find a function in the python lib, or message in the Crazyflie (depending how your system works) that only takes position but no attitude.

could be cool but actually I'm not using a crezyflie but i created a blimp with position control and the main problem that I'm facing is the yaw drift due to the inertia. paradoxically i would prefer a working yaw that a working x y and z. Thanks for the reply

[krichardsson]

I see. Sounds like a fun project!

By the way, if the problem actually is noise in the attitude data, it is also possible to add filters in the QTM software to change the output of the Qualisys system.

[Marcox2111]

I see. Sounds like a fun project!

By the way, if the problem actually is noise in the attitude data, it is also possible to add filters in the QTM software to change the output of the Qualisys system.

The problem is very systematic, I'm not using qualisys right now but I created a little script python that simulate qualisys data. and with raw perfect data it gives me a really specific error. I was working on a solution in these days and I think that I found the problem and maybe also the solution (not the best one). The problem is in the mm.pose.c file. In particular I think that there is a problem with the calculation of the error using the small angle approximation. I tried to find the precise problem but I failed. So I decided to try a workaround and it worked.

crazyflie-firmware/src/modules/src/kalman_core/mm_pose.c

Lines 40 to 46 in 2cf04d7

	// compute orientation error
	struct quat const q_ekf = mkquat(this->q[1], this->q[2], this->q[3], this->q[0]);
	struct quat const q_measured = mkquat(pose->quat.x, pose->quat.y, pose->quat.z, pose->quat.w);
	struct quat const q_residual = qqmul(qinv(q_ekf), q_measured);
	// small angle approximation, see eq. 141 in http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
	struct vec const err_quat = vscl(2.0f / q_residual.w, quatimagpart(q_residual));

from this line instead of use the quaternion approximation and yada yada, I converted into RPY and did the difference between the ekf and measured. As I said this is not a real solution but at least I can work properly with the yaw.

struct quat const q_ekf = mkquat(this->q[1], this->q[2], this->q[3], this->q[0]);
struct quat const q_measured = mkquat(pose->quat.x, pose->quat.y, pose->quat.z, pose->quat.

struct vec const att_ekf = quat2rpy(q_ekf);
struct vec const att_meas = quat2rpy(q_measured);
struct vec const err_quat = vsub(att_meas, att_ekf);

In this way it works (almost) perfectly (for my project).
This stuff still has some problem.
As expected when yaw is 180 or -180 (precisely just this point), it get crazy.
When roll or pitch goes over 60-70 degree, same. (this is not a big deal since the crazyflie is not a racing drone but still)

In the future maybe i could find a real solution to this bug. Thank you for the support

Obv these are all my guesses maybe the problem is somewhere else.

[krichardsson]

Interesting!
@whoenig I think you added this code. Does it sound plausible, insights or you have any input on this?

[whoenig]

It would be good to get a numerical example when the code fails. If the small angle approximation is indeed to blame, you can simply replace it with quat2rpy(q_residual) (see https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/interface/math3d.h#L679-L688, which is computational more expensive, but doesn't make any approximation).

The RPY idea is bad, because the difference of angles can be very wrong (i.e., the line err_quat = vsub(att_meas, att_ekf) is not correct in SO(2)) and could produce catastrophic failures that quaternion operations were supposed to avoid in the first place.

[Marcox2111]

Okay i can clearly see your point about my idea, at the end I knew was not a real solution.
I tried to remove the small angle approximation with the solution that you gave it to me and the results are literally the same as before so i guess that is not the small angle approximation the problem. I did some some other experiment just to give you some numbers but they change when the stdDev changes.

This is been took with stdDev of 1e-3. When the noise starts to be relevant is around 77 degree and it gets worse as long you increase the angle. At 90 the noise has a span of about 10 degree. At 120 degree the span of noise is almost 50 degree, At 160 degree, the state estimation is completely random, going from -150 to 150 randomly. The behavior is symmetrical so: starting from -77 and yada yada. (pitch and roll fixed to 0 and crazyflie on a table without moving, if i move it it recognize some movement but it's obiouvs)

[Marcox2111]

It would be good to get a numerical example when the code fails. If the small angle approximation is indeed to blame, you can simply replace it with quat2rpy(q_residual) (see https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/interface/math3d.h#L679-L688, which is computational more expensive, but doesn't make any approximation).

The RPY idea is bad, because the difference of angles can be very wrong (i.e., the line err_quat = vsub(att_meas, att_ekf) is not correct in SO(2)) and could produce catastrophic failures that quaternion operations were supposed to avoid in the first place.

I did some other test in the firmware to find where the error could be possible, and after many hours I didn't find what is not working but at least i excluded what for sure is working.
Moreover i printed the ekf quat and the "measured one", and i notice that it starts to diverge

eq0 0.800285, eq1 0.000003, eq2 0.000020, eq3 0.599619 
mq0 0.799160, mq1 0.000000, mq2 0.000000, mq3 0.601117 
eq0 0.789999, eq1 -0.000018, eq2 -0.000022, eq3 0.613107 
mq0 0.788547, mq1 0.000000, mq2 0.000000, mq3 0.614973 
eq0 0.778868, eq1 0.000019, eq2 -0.000018, eq3 0.627187 
mq0 0.777694, mq1 0.000000, mq2 0.000000, mq3 0.628642 
eq0 0.768139, eq1 -0.000010, eq2 0.000037, eq3 0.640282 
mq0 0.766605, mq1 0.000000, mq2 0.000000, mq3 0.642118 
eq0 0.756887, eq1 -0.000025, eq2 -0.000118, eq3 0.653544 
mq0 0.755281, mq1 0.000000, mq2 0.000000, mq3 0.655400 
eq0 0.744992, eq1 0.000294, eq2 0.000327, eq3 0.667072 
mq0 0.743728, mq1 0.000000, mq2 0.000000, mq3 0.668481 
eq0 0.733365, eq1 -0.001752, eq2 -0.000939, eq3 0.679831 
mq0 0.731948, mq1 0.000000, mq2 0.000000, mq3 0.681359 
eq0 0.721209, eq1 0.012111, eq2 0.002608, eq3 0.692606 
mq0 0.719945, mq1 0.000000, mq2 0.000000, mq3 0.694030 
eq0 0.710040, eq1 -0.101721, eq2 -0.003689, eq3 0.696764 
mq0 0.707723, mq1 0.000000, mq2 0.000000, mq3 0.706489 
eq0 0.713985, eq1 0.233391, eq2 -0.292064, eq3 0.591989 
mq0 0.695285, mq1 0.000000, mq2 0.000000, mq3 0.718733 
eq0 0.744789, eq1 0.382161, eq2 0.082239, eq3 0.540813 
mq0 0.682636, mq1 0.000000, mq2 0.000000, mq3 0.730758 
eq0 0.748089, eq1 0.166421, eq2 0.404852, eq3 0.498758 
mq0 0.669778, mq1 0.000000, mq2 0.000000, mq3 0.742560 
eq0 0.702521, eq1 -0.113826, eq2 0.420751, eq3 0.562561 
mq0 0.656717, mq1 0.000000, mq2 0.000000, mq3 0.754136 
eq0 0.684654, eq1 -0.280628, eq2 0.330647, eq3 0.585805 
mq0 0.643455, mq1 0.000000, mq2 0.000000, mq3 0.765483 
eq0 0.685283, eq1 -0.374943, eq2 0.223757, eq3 0.582868 
mq0 0.629998, mq1 0.000000, mq2 0.000000, mq3 0.776596 
eq0 0.699936, eq1 -0.433852, eq2 0.101051, eq3 0.558256 
mq0 0.616348, mq1 0.000000, mq2 0.000000, mq3 0.787473 

Obviously is something trivial but doing other tests with stdDev to 1.0e-1 i found that this noise reduces a lot (but everytime the device moves a little bit it doesn't trust anymore the pose) and the state estimation is not accurate.

At this point i think that the problem is in the kalmancoreScalarUpdate but i don't know if is just a limit of the kalman filter itself or of the implementation.

Thank you for all the support, if you have some idea on how to proceed or if you need some tests to confirm some hypothesis, it will be a pleasure to help

[krichardsson]

Unfortunately I don't have that much to add at this point.

[Marcox2111]

Unfortunately I don't have that much to add at this point.

I'm working on it, I found some critical parts and the performance are already improved a lot compared to the master, I think that is something that could be fixed, I will update you soon when i find enough to talk about it

[whoenig]

I don't think I have time anytime soon to dive into this, but my approach would be to collect event data on the uSD card and then use the Python bindings to analyze/debug the EKF on the PC.