82 replies to this topic

[cwabbott]

dschin, on 12 September 2010 - 07:16 PM, said:

Roy,

Sorry, I didn't reply sooner. But then again, a couple of days is actually pretty good for me . I spend very little time on the forums.

Here is a write up that I put together to early on for people like Peabody.  I'm sure it needs editing.  The intention was to polish it up towards the end of the summer as things got more solid on the OP INSGPS, and hopefully turn it into a paper for a conference or something (publish or perish!).  But alas here we are at the end of the summer (northern hemisphere).

As everyone continues to play with this keep a few of things in mind:

1) Many of the open source or DIY algorithms only estimate orientation and/or are targeted only at fixed wing, VTOL, outdoor, or indoor.  We attempting to find one solution that can be easily tailored to all but does the full INSGPS which is the right way to go if you are flying outdoors with GPS.

2) Much of the data that you see wandering around or jumping in some modes of the filter would not be used in simple stabilization modes or even be estimated by simpler algorighms.

Sounds cool! But, a couple things...

1. I think the derivative of position is velocity, not derivitive of velocity...

2. How do you deal with drift in baro altitiude due to local pressure changes?

3. Why did you decide not to model drift in the accels and magnetometers (for example, if throttle is changed resulting in a changing magnetic field)?

[peabody124]

cwabbott, on 12 September 2010 - 08:42 PM, said:

Sounds cool! But, a couple things...

1. I think the derivative of position is velocity, not derivitive of velocity...

2. How do you deal with drift in baro altitiude due to local pressure changes?

3. Why did you decide not to model drift in the accels and magnetometers (for example, if throttle is changed resulting in a changing magnetic field)?

I can answer some of those.  There is code to deal with the drift in the accels present but commented out.  The cost of adding states is pretty high, so we want to make measurements and confirm that drift is significant first.  I know currently the 6pt calibration doesn't do well enough for dead reckoning but I think that might be a bug on that side.  The baro drift is not treated well at all right now, it's on my todo list.  The first step thing will be to compute an offset between it and the GPS and then see how much that offset changes.

[dschin]

cwabbott, on 12 September 2010 - 08:42 PM, said:

1. I think the derivative of position is velocity, not derivitive of velocity...

2. How do you deal with drift in baro altitiude due to local pressure changes?

3. Why did you decide not to model drift in the accels and magnetometers (for example, if throttle is changed resulting in a changing magnetic field)?

peabody124, on 12 September 2010 - 10:14 PM, said:

I can answer some of those.  There is code to deal with the drift in the accels present but commented out.  The cost of adding states is pretty high, so we want to make measurements and confirm that drift is significant first.  I know currently the 6pt calibration doesn't do well enough for dead reckoning but I think that might be a bug on that side.  The baro drift is not treated well at all right now, it's on my todo list.  The first step thing will be to compute an offset between it and the GPS and then see how much that offset changes.

Not sure what 1. is referring to.  I didn't understand.

The baro will drift, especially if you have a weather front moving through.  We could add a state to estimate the bias, but Peabody is right on both the cost of adding states and the fact that its bias to GPS should first be initialized. I am hoping that we can just reset the bias periodically, without adding states.

Another reason for not adding back the accel biases yet is that with several modes (say indoor mode) I am fairly confident the filter may start to be in an unobservable state, where we don't have enough info to estimate all the things we are trying to estimate.  Also, I personally have very little hope of really doing deck reckoning where these biases would be the most important.  In working with several of these systems (some with better, more expensive sensors) and in talking to alot of other people who have developed them, no one claims to be able to do dead reckon past a few tens of seconds without GPS.  My simulations, which are more ideal than real life, also show it is nearly hopeless with the types of noises we get with inexpensive sensors. If someone has seen otherwise, please correct me.

We could also try to estimate scales for the sensors -- but that is out for sure right now.

[Jonathan Brandmeyer]

dschin, on 13 September 2010 - 01:39 AM, said:

Another reason for not adding back the accel biases yet is that with several modes (say indoor mode) I am fairly confident the filter may start to be in an unobservable state, where we don't have enough info to estimate all the things we are trying to estimate ...

We could also try to estimate scales for the sensors -- but that is out for sure right now.

Scale factor and bias calibration for the accelerometers should be fairly easy to perform off-line, and should be stable over the life of the product.  Is it something that can be performed by the "factory" or by the end user prior to installation in the host vehicle?

[Les Newell]

dschin, on 13 September 2010 - 01:39 AM, said:

Another reason for not adding back the accel biases yet is that with several modes (say indoor mode) I am fairly confident the filter may start to be in an unobservable state, where we don't have enough info to estimate all the things we are trying to estimate.  Also, I personally have very little hope of really doing deck reckoning where these biases would be the most important.  In working with several of these systems (some with better, more expensive sensors) and in talking to alot of other people who have developed them, no one claims to be able to do dead reckon past a few tens of seconds without GPS.  My simulations, which are more ideal than real life, also show it is nearly hopeless with the types of noises we get with inexpensive sensors. If someone has seen otherwise, please correct me.

I agree, these MEMS accels simply don't have the resolution needed for dead reckoning. Add in the vibration from a quad or engine powered plane and you are probably lucky to get more than a couple of seconds with any degree of accuracy.

[dschin]

Jonathan Brandmeyer, on 13 September 2010 - 02:45 PM, said:

Scale factor and bias calibration for the accelerometers should be fairly easy to perform off-line, and should be stable over the life of the product.  Is it something that can be performed by the "factory" or by the end user prior to installation in the host vehicle?

Yes, that is what I have heard before (the stable part).  I can't say I've ever tested it though.  But when you throw it in a circuit and read it with an A/D, is it still going to be true?

In the current 6 pt calibration in the GCS we are estimating scale and bias. As Peabody said though, we need to give that a going over again.  We don't estimate nonorthogonality of the axes though.  That's a little more involved (a nonlinear optimization and a little more confusing data collection process).  But it can certainly be done and may be done in the future.

[cwabbott]

dschin, on 13 September 2010 - 01:39 AM, said:

Not sure what 1. is referring to.  I didn't understand.

Sorry for being so confusing there... The error is in equation 5, of course this is because the paper is a rough draft only.

The thing to remember about correcting baro drift is that, if you decide to do it outside of the GPS/INS in a "cheaper" filter, possibly on the main board in the Altitude module (which is fine, this is what the ArduPilot people do), then the GPS altitude measurement becomes virtually meaningless (and thus you can speed up the filter...).

[peabody124]

cwabbott, on 13 September 2010 - 10:34 PM, said:

The thing to remember about correcting baro drift is that, if you decide to do it outside of the GPS/INS in a "cheaper" filter, possibly on the main board in the Altitude module (which is fine, this is what the ArduPilot people do), then the GPS altitude measurement becomes virtually meaningless (and thus you can speed up the filter...).

I'm not sure I follow.  It depends on the time scales of the changes.  If you know that offset change on the scale of minutes you don't need to update it at 125 Hz (new EKF speed thanks to some tweaks by Dschin).  The simplest thing to do would be another state space filter modeling their respective drift amounts (GPS fairly limited and drift on the order of seconds, baro less short term drift more long term drift) and then take the additional information into account from both in a very principled way.  There is no requirement for them even to run in the same filter although it's more straightforward to work out.

[cwabbott]

peabody124, on 13 September 2010 - 10:59 PM, said:

I'm not sure I follow.  It depends on the time scales of the changes.  If you know that offset change on the scale of minutes you don't need to update it at 125 Hz (new EKF speed thanks to some tweaks by Dschin).  The simplest thing to do would be another state space filter modeling their respective drift amounts (GPS fairly limited and drift on the order of seconds, baro less short term drift more long term drift) and then take the additional information into account from both in a very principled way.  There is no requirement for them even to run in the same filter although it's more straightforward to work out.

The reason that the GPS altitude would become useless is because of the same reason we use a barometer in the first place: the GPS altitude is noisy! Someone more knowedgeable could correct me here, but it seems to me that the GPS altitude is too noisy to be used in the INS/GPS altitude estimation to much effect, and therefore it's mostly used only to correct the baro altitude within the INS/GPS EKF. By using a seperate, simple lowpass filter or something similar to seperate the high-frequency GPS drift from the low-frequency baro drift, in addition to decoupling that estimation from the high-frequency EKF and getting a more simple/computationally inexpensive alternative, you can also remove a measurement input from the INS/GPS. So, basically, I agree with you.


Anyways, getting the EKF to run at 125 Hz is good news. From what I've read, you can't run your EKF on a quad at 40 Hz!

[dankers]

cwabbott, on 14 September 2010 - 01:19 AM, said:

Anyways, getting the EKF to run at 125 Hz is good news. From what I've read, you can't run your EKF on a quad at 40 Hz!

Just want to point out that not all filters are created equal.

These guys do ok with a 50Hz EKF



You must remember OP is about taking the state of the art forward! Hence a  GPS/INS solution and doing our own thing with massive amounts of testing. For me the great deal of fun in the project is how we are creating something new and testing the conventional wisdom, you'd be surprised how often it is not as wise as people think.

[Les Newell]

dankers, on 14 September 2010 - 02:17 AM, said:

Just want to point out that not all filters are created equal.
These guys do ok with a 50Hz EKF

You don't need the AHRS to be all that fast on a quad if you use the gyros directly for basic stability then the AHRS for orientation. FlightOS does this with two PID loops. The first controls the pitch/roll velocity using the gyros at a high update rate, the second controls pitch/roll position based on the AHRS at a lower update rate.This gives very good stability.

[peabody124]

Les Newell, on 14 September 2010 - 07:37 AM, said:

You don't need the AHRS to be all that fast on a quad if you use the gyros directly for basic stability then the AHRS for orientation. FlightOS does this with two PID loops. The first controls the pitch/roll velocity using the gyros at a high update rate, the second controls pitch/roll position based on the AHRS at a lower update rate.This gives very good stability.

How is that structured?  Is the control for the gyro PID a correction rate?

[dschin]

Les Newell, on 14 September 2010 - 07:37 AM, said:

You don't need the AHRS to be all that fast on a quad if you use the gyros directly for basic stability then the AHRS for orientation. FlightOS does this with two PID loops. The first controls the pitch/roll velocity using the gyros at a high update rate, the second controls pitch/roll position based on the AHRS at a lower update rate.This gives very good stability.

Les makes a very good point.  This could be even more important when we get the command-to-thrust bandwidth up on the ESCs.  The ESCs are also a limitation on control bandwidth, even with attitude rate controllers.

BTW I'm guessing I could stabilize a quad with just attitude control (no rate loops) and AHRS at 10 Hz. I just couldn't ask much of it, like rejecting ANY disturbance from the wind.

@cwabbott.  I think you've got the right idea on the GPS altitude and baro mixture. I think maybe the confusing part to Peabody was that you said it was useless, when maybe more specifically you meant useless in the the EKF algo if you've done the bias outside.  It is, as you said, still important for low frequency.  As I think about it though, it may be less costly to estimate the bias in the EKF. The way ours is set up it is fairly easy to change in and out measurements.  So if you have GPS use it. If you don't, you are stuck with the drift of the baro.  You also may only have GPS -- no baro.  Heck, you may add an ultrasonic sensor.  That's kind of the beauty of our EKF (and others), you use what you have when you have it.  It kind of takes care of the frequency stuff itself.  But if you try to do a complimentary filter outside the algo (GPS low freq, and baro high freq), then it gets messy managing it I think.  Don't know -- we have to think about it.

[dschin]

peabody124, on 14 September 2010 - 01:40 PM, said:

How is that structured?  Is the control for the gyro PID a correction rate?

Basically yes.  The attitude controllers' output would be an attitude rate command to the rate/velocity loop.  And then you could then probably get by with just P or PI on both the attitude and the rate loops.  The D in the attitude loop is like the P in the rate loop.

[Les Newell]

peabody124, on 14 September 2010 - 01:40 PM, said:

How is that structured?  Is the control for the gyro PID a correction rate?

Yes, exactly. The AHRS loop outputs a velocity command to the gyro loop. The gyro loop seriously damps out any instantaneous disturbances, such as custs of wind or turbulence.
The attitude loop uses P and I. The gyro loop is mainly P with a small amount of D

[peabody124]

Les Newell, on 14 September 2010 - 02:38 PM, said:

Yes, exactly. The AHRS loop outputs a velocity command to the gyro loop. The gyro loop seriously damps out any instantaneous disturbances, such as custs of wind or turbulence.
The attitude loop uses P and I. The gyro loop is mainly P with a small amount of D

How would that compare to sending the gyro directly out an using it as the D term in a single PID loop?  Also how fast is your inner PID loop running?

[Les Newell]

peabody124, on 14 September 2010 - 03:18 PM, said:

How would that compare to sending the gyro directly out an using it as the D term in a single PID loop?  Also how fast is your inner PID loop running?

I am not sure what you mean by 'sending the gyro directly out an using it as the D term in a single PID loop'. You could add the inverse of the gyro outputs to the output but that would fight any movement, including desired movement.

The gyro PID is running at 200Hz. The AHRS currently runs at 100Hz.

One advantage of having two PID loops is that it also gives you a 'manual' mode. An unstabilized multi-rotor, especially a small one, is virtually impossible to fly so even in manual mode you need some help. This is provided by the gyro loop. I see that the current OP code is truly unstabilized in manual mode.

[peabody124]

Les Newell, on 14 September 2010 - 03:33 PM, said:

I am not sure what you mean by 'sending the gyro directly out an using it as the D term in a single PID loop'. You could add the inverse of the gyro outputs to the output but that would fight any movement, including desired movement.

The gyro PID is running at 200Hz. The AHRS currently runs at 100Hz.

One advantage of having two PID loops is that it also gives you a 'manual' mode. An unstabilized multi-rotor, especially a small one, is virtually impossible to fly so even in manual mode you need some help. This is provided by the gyro loop. I see that the current OP code is truly unstabilized in manual mode.

That's a good thought, I may try that tonight.  I've had a few test flights in stabilized and it's _almost_ there I think.

[dschin]

peabody124, on 14 September 2010 - 03:18 PM, said:

How would that compare to sending the gyro directly out an using it as the D term in a single PID loop?  Also how fast is your inner PID loop running?

To answer the first question would require require a bunch of "if-thens", some of which I don't have the "thens" for without a little bit of analysis. But let's assume the context we are discussing is as follows: the attitude feedback rate/latency is the limitation on bandwidth for pitch/roll loop.  The limitation is not something that would also be part of the rate loop.  Then the PI-D (PI with derivative feedback) wouldn't be able to achieve any higher bandwidth for the first loop.  But the rate loop could get higher bandwidth because it doesn't have the attitude feedback latency, and is limited by something else like the ESC.  But for this to do us any real good we need the difference between the ESC limitation and the attitude feedback limitation to be significant, say a factor of ten on the achievable bandwidths.  

I don't think we ever want PI-D with a quad. IMHO opinion PI-D is only useful in some special cases. However, these special cases do coincidently arise sometimes in the roll and pitch loops of typical fixed wing aircraft.  But I don't believe that will be so in the quads. PID vs. PI-D with the same gains will give higher bandwidth control.  But it can cause nasty oscillations if the system has a bad resonance with a natural frequency just beyond the bandwidth. This arises from the aerodynamics in fixed wing roll and pitch loops sometimes.

To give a simple answer to the second question without much thought and being conservative, the inner "rate" loop should run at least ten times faster than the command-to-thrust bandwidth of the ESC-motor-prop system.  I have tested this bandwidth with several ESCs and the motor-prop combo on my quad.  It is about 2 Hz.  So 20 Hz on the rate loop would probably be ok for how my system is now (assuming we aren't sticking other large latencies from things like communications in there).

[dschin]

Les Newell, on 14 September 2010 - 03:33 PM, said:

You could add the inverse of the gyro outputs to the output but that would fight any movement, including desired movement.

dschin, on 14 September 2010 - 07:51 PM, said:

PID vs. PI-D with the same gains will give higher bandwidth control.  But it can cause nasty oscillations if the system has a bad resonance with a natural frequency just beyond the bandwidth.

We are saying the same thing here.  Les just said it more elegantly without a bunch of big words .  However, that same fight in movement is what prevents some things from being a problem in special cases.