Add supervisor for the kalman estimator

[krichardsson]

There is some code in the kalman estimator that caps the current state to stay within some bounds.

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

Lines 937 to 945 in 3112896

	// constrain the states
	for (int i=0; i<3; i++)
	{
	if (this->S[KC_STATE_X+i] < -MAX_POSITION) { this->S[KC_STATE_X+i] = -MAX_POSITION; }
	else if (this->S[KC_STATE_X+i] > MAX_POSITION) { this->S[KC_STATE_X+i] = MAX_POSITION; }
	if (this->S[KC_STATE_PX+i] < -MAX_VELOCITY) { this->S[KC_STATE_PX+i] = -MAX_VELOCITY; }
	else if (this->S[KC_STATE_PX+i] > MAX_VELOCITY) { this->S[KC_STATE_PX+i] = MAX_VELOCITY; }
	}

It seems as this causes crashes in some cases and we need a different solution.

The proposed solution is to introduce a kalman supervisor that resets the kalman filter if the state is off. One benefits of moving this into a new module is that it will be more obvious that the functionality exists, as opposed to the current solution where it is some what hidden in the kalman filter. Resetting the state instead of capping it should also be safer in terms of numerical stability.

[krichardsson]

This solution seems to work well in our flight arena.

I also added parameters to set the maximum position and velocity that will trigger a reset of the kalman filter. Setting these parameters to 0.0 or a negative number will turn off the check.

The only worry is what will happen in a larger system, we don't have the space to test it. One potential problem could be if a device is switched on far from the configured initial position (0, 0, 0 by default) and the filter starts to receive positioning data. When the estimated position changes a lot in a short period of time, the estimated velocity might exceed the accepted max velocity and trigger a reset. This process would repeat indefinitely but could be handled by setting the max velocity to 0.0.

[ntamas]

When the estimated position changes a lot in a short period of time, the estimated velocity might exceed the accepted max velocity and trigger a reset. This process would repeat indefinitely but could be handled by setting the max velocity to 0.0.

This is what could have happened with us today (see issue #506). Our arena was 18m x 12m x 6m (we were trying to test how large an arena we could cover with our UWB anchors), and the CF was placed near the far end of the arena around (17.5, 11.5, 0).

[krichardsson]

@ntamas that sounds like a plausible explanation for #506. The "correct" solution to this problem is probably to set the initial position in the kalman filter (kalman.initialX, kalman.initialY, kalman.initialZ) to a position close to the actual CF location and then reset the kalman filter.
Not sure this is a practical solution in real-world situations though?

[ntamas]

FWIW I think that rate-limiting the Kalman filter reset attempts would also be a solution - it would still reset the Kalman filter once when the first position estimate arrives, but then it will back off for a short period, giving enough time for the Kalman filter to stabilize around the new position.