Minimal EKF Implementation

I’m trying to get a minimal implementation of the ECL EKF working - so I’m sending it gyro, accel, mag, and baro data only. It appears that the pitch, roll, and yaw estimation is working, but the altitude is always tending towards zero, not towards the baro altitude. If I move the copter vertically, the altitude output tracks my movement, but then immediately starts heading back towards zero.

Any suggestions about what would make the EKF always push the altitude towards zero? I’ve verified that the baro data I’m sending is correct.

Probably the easiest way for you to debug this is the system wide replay feature of PX4. Have a look at the documentation, there is a section that shows how to replay ekf2. Like this you can.put printfs in the code and figure out what happens to the baro data.

Otherwise have a look at the estimator_status and ekf2_innovations message. The former one gives you information about the solution status. You can see the meaning in the definition of the orb message. For baro you need to look at ekf2_innovations.vel_pos_innov(5)

Hi @tumbili, thanks for the quick response. I should clarify that this is a from-scratch implementation of a flight controller in C, using the ECL EKF - so not based on PX4. That being said, I do have logging set up for the innovations, biases, and status data, and I can use the debugger to step through the kalman filter and my interface code. It is showing a high rate of constant z-axis position innovations, even when sitting still.

I’ve also made some progress with this problem, or at least found another useful data point: if I take the drone outside so that I get GPS lock; the altitude immediately jumps to the GPS altitude, and then it appears that GPS, Baro, and Inertial data is being merged for altitude. However it looks to me like it’s relying on GPS more than it should - I’ve got a really good, consistent baro altitude, yet the estimated altitude is wandering a lot, presumably based on my GPS altitude.

I wonder if it’s doing the same thing indoors, when it doesn’t have GPS lock? Assuming a GPS altitude of zero and merging that into the altitude estimate?

Obviously you don’t have the code to look at. If it helps I can upload my interface code and some graphs of the altitude input/output/innovation charts.

Hi, I’m curious about your “from scratch implementation of a flight controller in C”. Would you mind sharing some info about it? I’m currently trying to do something similar but I can’t quite understand how to do it. For now I’m just adding an extra app to the default build that does what I need to do, but ideally I would like to have my minimal flight controller flashed on the Pixhawk.

Thanks!

Hi, sorry for the delayed response. I’m working on a from-scratch flight controller for my university, so all custom hardware and software.

The board is similar to the pixhawk 1 - an STM32F4 main processor and an STM32F0 I/O or failsafe processor. Mostly the same sensors too - an MPU9250 IMU, and MS5611 barometer for example.

On the software side, we use ChibiosOS, our own device drivers, and (up to now) our own flight control code with our own GCS. We’re still continuing with our flight control system, but we’re replacing the old AHRS with the EKF from PX4 to get the full-state estimation that’s necessary to fly multicopters well.

The integration with the EKF has been challenging, but successful. Despite being posted as a stand-alone project, it’s evident that the EKF is really only being used with the PX4 right now. For example, the “getting started” instructions in readme.md for building the stand-alone library don’t work. Some of the functionality it has, like emitting error messages, only work when built with the PX4. The stand-alone version is hard-coded to emit error messages through printf(), which isn’t great on systems that don’t have a stdout. That being said, it can be made to work, and it gives good results (as you see in PX4 copters and airplanes) when set up correctly.

To get started from a programming standpoint, you need to include ekf.h and then create an instance of an Ekf(). The use the methods in estimator_interface.h to push all of your sensor data to the system. Then call Ekf->update(), then use more estimator_interface.h methods to read the results back. Simple in theory, but there are gotcha’s in every part. PX4’s connector (https://github.com/PX4/Firmware/blob/master/src/modules/ekf2/ekf2_main.cpp) is the reference example to follow.

It’s also important that all of your sensor data is as perfect as it can be - so if you do your own device drivers you need to test all of them thoroughly for data rates, calibration, noise levels, aliasing, etc. Due to the nature of the EKF, when bad data goes in, bad data goes out but in seemingly random ways that are hard to track back to a source.