Is there a need for a kalman filter to fusing altitude and position?

[Thomas Larsen]

Hello all.

I have been experimenting lately with a kalman filter for the Ardupilot code to learn the code and hopefully contribute to the development. Currently I have written a custom log message which include all measurements necessary to fuse the vertical channel. Then the log is imported to Matlab where i have written a simple kalman filter to fuse the accelerometer and barometer measurements. would this be useful?

Another though was to do the same with the horizontal position. I think if the Horizontal dilution of precision estimate from the GPS is used in the measurement covariance, the effects of a GPS glitch might be less sever. 

Here are some plots for the kalman filter performance. It is not a very good increase in performance, but the barometer is already filtered with an moving average filter in the ardupilot code which can be replaced by the kalman filter. 

[Jonathan Challinger]

Altitude is not a great example - baro is already very high performance compared to GPS, and the acceleration measurement is much more accurate because it doesn't have to cope with the (relatively large) heading errors - only attitude errors which are a couple of degrees. As a result, the complementary filter we're using on the Z axis works very very well and quite reliably.

The difficulty is horizontal position - I think the best results will come from fusing three measurements: the accelerometers, the GPS, and an estimated acceleration from lean angle.

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/groups/opt_out.

[Randy Mackay]

Thomas,

     I think we're interested in looking at an alternative to the complementary filter if you've got one.  I think the important thing we need is more resistance to vibrations.  So for example if you pushed random (or perhaps biased) noise into the two filters does the kalman filter perform better?  ...and would it be possible for the filter to tell us when it thinks it's not doing well?  I've heard the covariance matrix gives this information but frankly I don't know much about kalman filters yet.

-Randy

---

From: Jonathan Challinger <mr.cha...@gmail.com>
To: drones-...@googlegroups.com
Sent: Sunday, September 1, 2013 6:55 AM
Subject: Re: [drones-discuss] Is there a need for a kalman filter to fusing altitude and position?

[Leonard Hall]

Hi Thomas,

The short answer is yes :)

I have also played with Kalman filters for tracking of radar targets but in that case it is very simple. The bit I don't know how to deal with is the delay in the GPS or BARO measurement. The other thing that messes us up is the heading error when translating accelerations to position in the inertial navigation. The information is there to correct but I have no idea how to approach the problem using the Kalman filter.

Thanks,

Leonard

[Joonas Melin]

Hello,

The thing with Kalman filters (as many of you probably know) is the modeling, so it all depends on your model and what you have on your state.

To keep your heading from messing up the global accelerations, you need to have at least your heading as your state variable (on top of position and /or accelerations). Just quickly thinking through you would need to have your accelerations at global frame, with local heading. My  quick hunch would be that you would need to have this as an EKF so your model can be non linear. Your measurement model would now describe how your state, consisting of accelerations and heading can be translated to your measurement of position (this involves a rotation matrix for the heading and an integration with the sample time). After you get your measurement model, you need to make and aproximate linearization of it by calculating the jacobian matrix (partial derivatives), now this jacobian is the one that takes care of your heading. The jacobian is inverted at one point so this describes how the difference in position described by your state and the position you measured, will affect your state, and if your models are correct, it will twist your accelerations and heading to correspond with the position measurement, and your position measurement to correspond more to your accelerations and heading (depending on the covariance of your state and measurement of course).

I would guess that you dont need to have all position,acceleration,velocity and heading on your state since much of these can be calculated from each other, so this would make your matrices smaller, but I havent tought this through quite that well so some of them might be required after all, but once again, depends on your measurement and system models.

As for the time lag, I tested a solution where measurements and predictions were saved until new measurement was fetched. As we had around 400ms of lag on our GPS, I fused the GPS measurement with 400ms old prediction, and then just propagated the prediction steps from that time to get the current state. This is of course an brute force method that basically just simulates the delayed measurement. Maby it would be possible to just combine all the previous predictions as one prediction step? this might make it actually feasible to calculate on limited processing power.

[Thomas Larsen]

I don't see why heading should need to be estimate? In my understanding the current AHRS does a good job on estimating the attitude and I think that running a full AHRS with a non linear kalman filter will run to slow for the APM. What I am suggesting is a simpler linear kalman filter to improve the quality of the positioning and leaving the AHRS as it is.

The plot above shows an simple example of with fusing of the barometer and accelerometer on the APM 2.0. The difference will become more apparent when the low pass filter for the barometer is turned off, but by studying the graph on my computer I see a slight improvement.

My goal for this weekend is to do the same with the GPS position and accelerometers to see how the filter handles a GPS glitch.

[Leonard Hall]

Heading shouldn't need an estimate but because the AHRS doesn't get it right when there are compass offsets and we get toilet bowling. I am pretty sure that the correct compass offset could be derived from the difference between the accelerometer and the gps position. I was wondering if you had any experience with a similar problem.

As I said before, my understanding of the Kalman  filter is pretty thin. I can use them to do simple acceleration, velocity, position type filtering on a single axis however I haven't tried to add an offset. However my major hole in my understanding is how to efficiently include a delay of the measurement into the system. How are you handling that problem here?

Good work,

Leonard

[Ritchie Wilson]

As we don't have the head room on the APM for a kalman filter (as has been discussed many times) you might want to ask James Goppart who is doing the work for ETH with Meier et al on state estimation for the PX4

Regards

Ritchie.

[Brandon Basso]

Leonard,

Kalman filtering with intermittent/delayed measurements has been solved, and is actually implemented in lots of places from what I understand, so it's not just pie in the sky research.  There's the link to the original theory, though i'm sure there are mroe practical references out there since 2004.

I'm a big fan of the Unscented Kalman Filter, aka Sigma Point Kalman filter.  It has been used for small aircraft applications and has some really nice computation and storage features. Definitely check out this paper where the implementation on trad heli is described in details, it's a great read. 

The UKF is mainly an EKF under the hood but does not require the calculation of Jacobians, which is the part that almost certainly can't happen on APM.  You instead pick a few test points in your state space and propagate them forward through your (nonlinear) dynamics. More points  = more accurate representation of your covariance matrix, so it's a lot like a particle filter in that you have a nice slider between accuracy and computational efficiency.

-Brandon

--
Brandon Basso, PhD :: Senior Research and Development Engineer :: 3D Robotics :: Berkeley, CA

[Leonard Hall]

Brandon, Nice reply sir!

Thanks mate, I will have a good read :)

[Randy Mackay]

     I think because the pixhawks is now out and we're investigating the gps-glitch problem that it's a good time to look into a kalman filter especially for the combination of the gps + accel based position estimate.  So if Thomas, or someone else has a somewhat working kalman filter that we could experiment with and perhaps try to replace the existing complementary filter with (on the px4/pixhawks) please ping me.

-Randy

---

From: Brandon Basso <bra...@3drobotics.com>
To: drones-...@googlegroups.com
Sent: Friday, September 6, 2013 2:44 AM
Subject: Re: [drones-discuss] Re: Is there a need for a kalman filter to fusing altitude and position?

[Jonathan Challinger]

Does Paul Riseborough still work on ardupilot? He knows a lot about kalman.

[Meier Lorenz]

Hi all,

Here is the Position + Att Kalman filter James Goppert did for PX4:

https://github.com/PX4/Firmware/blob/master/src/modules/att_pos_estimator_ekf/KalmanNav.cpp

It does even calculate GPS jamming / error metrics already and updates accordingly, and it would very likely a very solid base to build on.

-Lorenz

On 17 Sep 2013, at 07:41, Jonathan Challinger <mr.cha...@gmail.com<mailto:mr.cha...@gmail.com>> wrote:


Does Paul Riseborough still work on ardupilot? He knows a lot about kalman.

On Sep 16, 2013 8:18 PM, "Randy Mackay" <rmac...@yahoo.com<mailto:rmac...@yahoo.com>> wrote:

I think because the pixhawks is now out and we're investigating the gps-glitch problem that it's a good time to look into a kalman filter especially for the combination of the gps + accel based position estimate. So if Thomas, or someone else has a somewhat working kalman filter that we could experiment with and perhaps try to replace the existing complementary filter with (on the px4/pixhawks) please ping me.

-Randy


________________________________

From: Brandon Basso <bra...@3drobotics.com<mailto:bra...@3drobotics.com>>
To: drones-...@googlegroups.com<mailto:drones-...@googlegroups.com>

Sent: Friday, September 6, 2013 2:44 AM
Subject: Re: [drones-discuss] Re: Is there a need for a kalman filter to fusing altitude and position?

Leonard,

Kalman filtering with intermittent/delayed measurements has been solved, and is actually implemented in lots of places from what I understand, so it's not just pie in the sky research. There's the link to the original theory<http://users.ece.cmu.edu/~brunos/Publications/tacs04.pdf>, though i'm sure there are mroe practical references out there since 2004.

I'm a big fan of the Unscented Kalman Filter<http://backup.lara.unb.br/~gaborges/disciplinas/efe/papers/wan2000.pdf>, aka Sigma Point Kalman filter. It has been used for small aircraft applications and has some really nice computation and storage features. Definitely check out this paper<http://www.cse.ogi.edu/~rudmerwe/pubs/pdf/GNC2004_spkf.pdf> where the implementation on trad heli is described in details, it's a great read.

The UKF is mainly an EKF under the hood but does not require the calculation of Jacobians, which is the part that almost certainly can't happen on APM. You instead pick a few test points in your state space and propagate them forward through your (nonlinear) dynamics. More points = more accurate representation of your covariance matrix, so it's a lot like a particle filter in that you have a nice slider between accuracy and computational efficiency.

-Brandon

On Thu, Sep 5, 2013 at 12:05 AM, Ritchie Wilson <originaln...@gmail.com<mailto:originaln...@gmail.com>> wrote:
As we don't have the head room on the APM for a kalman filter (as has been discussed many times) you might want to ask James Goppart who is doing the work for ETH with Meier et al on state estimation for the PX4

RegardsRitchie.

On 5 Sep 2013, at 00:00, Leonard Hall <leonar...@gmail.com<mailto:leonar...@gmail.com>> wrote:

Heading shouldn't need an estimate but because the AHRS doesn't get it right when there are compass offsets and we get toilet bowling. I am pretty sure that the correct compass offset could be derived from the difference between the accelerometer and the gps position. I was wondering if you had any experience with a similar problem.

As I said before, my understanding of the Kalman filter is pretty thin. I can use them to do simple acceleration, velocity, position type filtering on a single axis however I haven't tried to add an offset. However my major hole in my understanding is how to efficiently include a delay of the measurement into the system. How are you handling that problem here?

Good work,
Leonard

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com<mailto:drones-discus...@googlegroups.com>.

For more options, visit https://groups.google.com/groups/opt_out.
--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com<mailto:drones-discuss%2Bunsu...@googlegroups.com>.

For more options, visit https://groups.google.com/groups/opt_out.



--
Brandon Basso, PhD :: Senior Research and Development Engineer :: 3D Robotics :: Berkeley, CA
--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com<mailto:drones-discuss%2Bunsu...@googlegroups.com>.

For more options, visit https://groups.google.com/groups/opt_out.



--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com<mailto:drones-discuss%2Bunsu...@googlegroups.com>.

For more options, visit https://groups.google.com/groups/opt_out.

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.

To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com<mailto:drones-discus...@googlegroups.com>.

[Thomas Larsen]

I have made i filter for both horizontal and vertical navigation in Matlab, but I will need a couple of weeks to port it over to test it properly and to convert it to the APM code. From what I have observed till now, the filter improves the existing solution if the AHRS solution is valid and can help to compensate for a GPS glitch or misalign magnetometer. I did some tests laps in a parking garage with the APM 2.0 in a parking garage and it shows that it has the potential to estimate position with <1 meter precision with poor GPS reception. I I will post more when I have something to show for. 

[Niels Joubert]

Hi guys!

I'm trawling through the AP_InertialNav.cpp and AP_AHRS_DCM.cpp code at the moment, and I don't see any place where the barometer and GPS altitude data is fused.

In AP_InertialNav.cpp, _position is set as _position_base + _position_correction. (line 90)

_position_correction's z component is updated from _positon_error.z (line 81)

_positon_correction.z is set only from the barometer in correct_with_baro (line 328)

In AP_AHRS_DCM.cpp, the GPS's velocity is pulled inside drift_correction() on line 551 and constructed into a "velocity" Vector3f. The x and y components are used a bunch, but the z component is not. The entire velocity vector is used on line 598, 599:

  Vector3f vdelta = (velocity - _last_velocity) * v_scale;

        GA_e += vdelta;

this is the only place where the GPS appears to contribute to something. From here it propagates further.

Where is this complementary filter for positioning implemented? Is the position of the quad a fused value of the GPS and Baro readings or it is only the Baro?

[Thomas Larsen]

In the bottom of the ardupilot.pde file (and probably the arducopter as well) you will find the class static void update_alt().

[Niels Joubert]

Ah!

Interestingly enough, in ArduPlane it is exactly as you say! update_alt() contains this jewel:

    if (barometer.healthy) {

        // alt_MSL centimeters (centimeters)

        current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude_cm;

        current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);

    } else if (g_gps->status() >= GPS::GPS_OK_FIX_3D) {

        // alt_MSL centimeters (centimeters)

        current_loc.alt = g_gps->altitude_cm;

    }

On the other hand, in ArduCopter, there is no update_alt(), but there is an update_altitude():

// read baro and sonar altitude at 20hz

static void update_altitude()

{

#if HIL_MODE == HIL_MODE_ATTITUDE

    // we are in the SIM, fake out the baro and Sonar

    baro_alt                = g_gps->altitude_cm;

    if(g.sonar_enabled) {

        sonar_alt           = baro_alt;

    }

#else

    // read in baro altitude

    baro_alt            = read_barometer();

    // read in sonar altitude

    sonar_alt           = read_sonar();

#endif  // HIL_MODE == HIL_MODE_ATTITUDE

    // write altitude info to dataflash logs

    if ((g.log_bitmask & MASK_LOG_CTUN) && motors.armed()) {

        Log_Write_Control_Tuning();

    }

}

It would appear that the APM:Copter codebase doesn't do this complementary filter the way APM:Plane does! Can someone else confirm this too?

[Ben Nizette]

Hi Niels,

I’ve done some work in this area recently, you’re quite right that Plane and Copter are different. Simply, they have different goals: Copter is set up to loiter in a small box, while such behaviour doesn’t make sense for Plane. The APM doesn’t really have the CPU cycles to implement anything more generic (for example the Kalman filter in the subject line), but there is some work in progress to get a single, proper solution on PX4-class boards and above. I think I even saw something to this effect in the minutes of the last dev call.

Specifically for copter, altitude is baro fused with accel while x/y is GPS fused with accel.

I issued a PR last night that incorporates GPS in to the Z axis as well, you should be able to view the request on the main DIYDrones Github page or look at the gps_mixing branch on http://github.com/benizl/ardupilot. Note that this treats GPS and baro the same as each other in the Z axis where as in fact they have quite different noise models (one’s good short term but drifts, the other is good long term but glitches); in that sense it’s far from optimal. But it does what I need specifically and as I say, there’s a more generic solution in the works for boards that can handle it.

HTH.

—Ben.

[Niels Joubert]

Hi Ben!

Awesome! That's what I was thinking of as well! 

See, I've got a a couple of RTK GPS board that gives me centimeter-accurate GPS positioning (they were mad expensive haha!) and I'm working on integrating them into the APM code. I'm targeting the Pixhawk with this, and longer-term I imagine some form of unscented Kalman filter is the way to go, but as a first cut I want to just feed the GPS data into the AHRS and Inertial Nav system. These GPS units should be better than any of the other sensors on the board, and would be worth fusing for altitude.

What's the caveats with that you're doing? I took a super-quick peek at the commit you posted and it seems pretty straight-forward fusing the height of the barometer and GPS, which is what I was thinking of doing anyway!

Cheers!

-Niels Joubert

Stanford Ph.D Candidate

[Ben Nizette]

Great! What we’re doing specifically is flying quads for factory inspection (external) where the baro gets fairly munted flying through steam, smoke, near hot stacks etc. We’ve got a decent GPS, not RTK but with a good antenna we still get sub-metre vertical performance which does the job. My patch really just gives tuning knobs to tell APM how much you trust the dynamics of each of your sensors at any given time.

As for caveats, well, the main limitation is as I say that really you’d use the GPS to compensate for baro drift where as this patch is only really good for smoothing the shorter term dynamics of each other, not the long term trends. Another PR from last night adds proper baro noise models to the SITL simulator so feel free to play around with the behaviour in there to see what I’m talking about.

If you’re targeting PX4, Pixhawk then you might like to get in touch with Paul Riseborough (if he’s lurking?) and see what the state of his Kalman work is for that class of boards. Or just use the PX4 native firmware which has substantially different inav (though I haven’t looked at it in detail to check that it does what you want).

—Ben.

[Paul Riseborough]

Thomas,

I finally got enough contiguous time on it to put together an EKF that I am hoping will meet both plane and copter requirements. It estimates all of the things we are interested in  (gyro bias, acc bias, magnetometer corrections) and enables the effective delay for each measurement source (relative to t he IMU data) to be specified. It requires ~1.6MFLOPS to run with all sensors used. Unfortunately its still only in Matlab/Simulink, but the conversion process to C++ has started.

https://github.com/priseborough/InertialNav

I've replayed plane data through it with good results, and am now looking for some suitable copter flash logs if someone has them.

Cheers,

Paul