Page 1 of 1

[solved] Custom flight stack with Crazyflie Kalman Estimator

Posted: Fri Mar 08, 2019 4:36 pm
by OmarMorando
Hi everyone.

I implemented some parts of your firmware in my custom flight stack based on ChibiOS/RT and a Teensy board with MPU9250, in particular the Kalman estimator that Mark made for Crazyflie.

I have a strange behavior. The attitude is calculated correctly during the movement, the indicator follows the angles very well. However, when I stop moving the flight controller, the angles of roll, pitch and yaw slowly tend to zero even if I keep the flight controller tilted (eg 30° roll and 15° pitch).

The IMU is read correctly, if I replace EKF estimator with the Mahony filter everything works fine, if I keep the flight controller with certain angles the indicator stays in position.

Could you give me some tips to figure out what am I doing wrong? Is it a different orientation problem than my IMU compared to Crazyflie?
I know this question is a bit off topic, but I would just like to know if it has already happened to you and if you have any advice.

Thanks anyway.

Re: Custom flight stack with Crazyflie Kalman Estimator

Posted: Mon Mar 11, 2019 12:40 pm
by tobias
If I remember correctly the Kalman filter has a "bleed" effect towards zero. Since it is based on physics and that it should be "flying" the scenario you describe can't really apply, because then it is stationary. So I believe the Kalman filter works as designed.

Re: Custom flight stack with Crazyflie Kalman Estimator

Posted: Mon Mar 11, 2019 1:12 pm
by OmarMorando
Hi Tobias,
I don't know if I have interpreted well what you said, in any case if I move CF2 and put it in an inclined static position, the indicator follows the movements and then stays with the pitch and roll values fixed according to the position of the drone. If I do the same thing with mine, the indicator, as soon as the drone stops, tends to have all the values at zero in 4-5 secs.

The impression I have is as if it's using only the gyroscope and not the accelerometer, as soon as the drone is stopped the angular velocities are zero and consequently the result progressively tends to zero. Acc values are passed in the right way (I hope...).

I keep testing and if I find the solution I will publish it, in case it may be of general interest.
Thanks again.

Re: Custom flight stack with Crazyflie Kalman Estimator

Posted: Tue Mar 12, 2019 1:07 pm
by OmarMorando
Fixed: the problem was related to ROLLPITCH_ZERO_REVERSION param, not correct for my drone.

Re: [solved] Custom flight stack with Crazyflie Kalman Estimator

Posted: Wed Mar 13, 2019 11:07 am
by tobias
Great! Would be nice to know more about your drone. Maybe we can learn something :D.

Re: [solved] Custom flight stack with Crazyflie Kalman Estimator

Posted: Wed Mar 13, 2019 2:17 pm
by OmarMorando
On the contrary, it is me who have learned a lot from your code.

For a project I'm working on of swarm of drones for indoor flight, I decided to make a new flight controller based on the Freescale MK66FX1M0 processor and Teensy bootloader, basically a custom version of Teensy 3.6 with 36x36 mm dimensions, Bosch BMI088, BMP380 and BMM150 sensors, optical flow, ranging.

I'm currently using a Teensy 3.2 with MPU9250 and MS5637, waiting for the custom board to be built with Teensy 3.6. The big limitation of the Teensy 3.2 processor is the low performance level, especially for processing the EKF task which alone takes about 2.5 ms per cycle.

About software, I'm writing the flight stack starting from scratch, based on ChibiOS/RT. For some parts I was inspired by your code (in particular the EKF), other things have been developed ad-hoc (eg PID controller). The choice to use your EKF starts from the fact that I'm integrating optical flow, tof ranging and UWB localization. In terms of communication, I implemented a custom version of the MSP protocol (Multiwii Serial Protocol), this because I need to add a ROS-based companion computer that I already had the communication part made. The swarming algorithms will run on the companion computer which will pass the commands to the flight controller using MSP.

Current state:
- scheduler and tasks: done
- IMU, baro, compass, opt-flow, ranging: completed
- PPM decode, ESC, UARTs, SPI, I2C: completed
- MSP send/receive commands: completed
- flight modes: 90% completed
- the drone is able to fly with a good level of stability (autonomously)
- integration of opt-flow and UWB to be completed (WIP)
- a long list of bugs to fix ...

At this point your question, Tobias, could be: why don't you use our RZR control board directly? I'm one of the lucky owners of this board, which I've installed on another drone and which I use as a term of comparison for what I have to accomplish.
The reasons are two: given the delicate aspect of the project I wanted to know exactly what was behind the code, and the best way was to write it from scratch. Second, it was the best way to learn.
As a consequence I had to study the CF2 code very carefully, some parts of it have been implemented or adapted with some changes, and this has allowed me to know it quite well.

Images of the current card (Teensy 3.2 and opt-flow + ranging)
https://drive.google.com/open?id=1HXM47 ... XJ7BNQJlNp
https://drive.google.com/open?id=1_0Apb ... _6RMXcN2M9

Re: [solved] Custom flight stack with Crazyflie Kalman Estimator

Posted: Thu Mar 14, 2019 1:22 pm
by OmarMorando
I have another question.

Using the EKF and keeping the drone still on the desk I have a very slow but constant drift on roll and pitch (about 0.1 °/min), after several minutes the error exceeds 10-15 °. This doesn't happen if I leave everything unchanged and use Mahony.

I tried to play a bit with the acc and gyro calibration procedures, using both yours and new ones but it doesn't change, what varies is the drift error (speed).

Any suggestions?

Re: [solved] Custom flight stack with Crazyflie Kalman Estimator

Posted: Mon Mar 18, 2019 1:20 pm
by tobias
First thing, thanks for you detailed post, very interesting to read about. I hope to read more as you move forward.

Regarding your question I would expect the EKF to slowly come back to zero as that is how I recall it being designed. But you are seeing it drifting away from zero?

Re: [solved] Custom flight stack with Crazyflie Kalman Estimator

Posted: Mon Mar 18, 2019 2:13 pm
by OmarMorando
It slowly reaches zero, but then continues with a slow drift and overcome zero angles. If I larve stable the flight controller for several minutes, the angles reach 20-30° or even more.

Re: [solved] Custom flight stack with Crazyflie Kalman Estimator

Posted: Tue Mar 19, 2019 1:26 pm
by OmarMorando
This is the log graph with a sampling of 0.5 sec (790 points for a total of 395 sec), with the following values calculated at the beginning and then constant:
- gyroBias.x = 4.006
- gyroBias.y = -28.867
- gyroBias.z = -9.547
- accScale = 1.052

https://drive.google.com/open?id=1wqMa2 ... 2KoiYGDiIe