1 reply to this topic

[peabody124]

So something Kenz brought up on IRC which I think would be a good idea is to have a standardized API for attitude estimation filters.  However, my concern is that really we'd spend so much time trying to generalize things and have configuration options based on sensors and timing it wouldn't do much good (although the math should still be abstracted out of the code).  So towards that end I'm looking for some suggestions on how to architect this cleanly.

Currently the INS algorithm is largely in it's own file, but there's still a fair bit of logic about pushing the data into and modes scattered around ahrs.c.

Broad strokes: we have a list of possible sensors (accel, gyro, mag, gps, pressure, optical, ultrasound) that in principle can all be available at different times.  For each cycle (defined as when there will be an update of the state estimate) we could have a set of functions

filter.set_gyro_data(float *)
filter.set_accel_data(float *)
...
filter.step()
filter.get_attitude(float *)
filter.get_position(float *)
...

and the filter could know what to do based on the data set since the last step.  However in the ahrs right now I currently do some processing to export the data ASAP between the prediction and correction step and a general "step" function would prevent that...

[Kenn Sebesta]

Just to add a little more precision and explain my motivation, basically when looking through the code, I realized that the CopterControl attitude control is heavily dependent on the hardware. This makes future maintenance difficult, as a version 2.0 of the CC, or any other spinoff for that matter (such as the MoveCopter), requires extensive retesting of the stabilization algorithms.

My vision is to completely separate the filter from the measurements. The filter tuning is sensor based, the basic structure of the filter is not. Furthermore, in a more general sense, the filter is independent of sensor availability. Note, this statement needs to be understood in the context of redundancy of sensor data. In theory, a GPS would be enough to completely stabilize a moving vehicle, but the reality is that the quality of information and the measurement frequency are far too low for this. That's why we use other sensors, such as accelerometer, gyroscope, pressure, magnetometer, ultrasound, etc... in order to have a practical application.

However, each model might have a slightly different set of these sensors. There's no reason why a sufficiently good magnetometer couldn't replace the gyroscope, or perhaps the accelerometer. We don't do it... yet. But there's no reason not to write code that allows this to occur in the future, assuming that said code modularity does not impose unbearable costs.

Continuing the discussion, the measurements do not even need to be synchronous. In fact, they're not, we just like to pretend they are because it makes our code easier to read (but not necessarily faster nor more efficient). There's absolutely nothing wrong with a gyroscope that updates at 123Hz coupled with an accelerometer that feeds data in at 3.5kHz. All this can be handled directly by the filter, the mathematics do not care. The tuning does, but not the algorithm.

The way this happens is that there is a background loop that is constantly updating the state estimation. Always and forever, and without any input whatsoever from outside functions. It runs as its own RT task, and can even have the integration step size be constantly changing (within bounds, of course) to suit the current loads on the processor. Whenever a new measurement-- of any kind-- arrives, a flag is thrown and the filter knows to grab the most current state estimation. It applies the correction and then goes back to mindlessly integrating.

The control update itself happens at a fixed frequency, whatever is suitable for the actuators. If the ESCs are updating at 400Hz, then this would be the control update frequency. This actually leads to faster and mroe up-to-date control, as the processor does not spend time calculating a new state prediction before applying the update.

Obviously, different sensor data will affect different state estimates. In a Kalman Filter, this is trivially easy, and I believe is in many ways similar to the current ahrs.c code. So OP-AHRS really won't have a problem with this.

In a complementary filter, which you find in the OP-CC, there are a couple more nuances of how sensor data should affect state estimates that are loosely related, such as barometer data giving you a weak feedback on aircraft pitch, but even if we ignored these weak relations, we wouldn't be doing any worse than the current code. (Not to imply that it's bad. It's actually quite good. Rather, I'm establishing a floor for a worst-case scenario.)