38 replies to this topic

[PeterG]

Lewcifer, on 24 May 2010 - 06:21 AM, said:

By "repeatable testing standard" I mean the ability to create and execute a standardized and thorough test with repeatable accuracy.  I can then put the device through a hundred iterations or so, and observe disparities in trajectory (and sensory) data.  The test would include various acceleration rates, at various angles (and multiple attitude translations throughout).  I don't want to do the standard thing of rotating it in my hand about the various axes; I want the test itself to be more robust.  I'm also not sure of how to accurately measure or even ascertain the "correct" answer (as far as sensory outputs) to the test.  Therein lies the dilemma.

Of course, the above has to be repeated for the different filtering techniques under test.  Producing a test with repeatable accuracy and known correct measurements along its entire path... that's something I don't yet know how to handle.

What is your budget? The easiest you can do is probably to convert an old turntable, of course this will only give you a uniform circular motion.

[Bani Greyling]

as I went to sleep last night, I realised that's probably what you are after.

Maybe a simplified test rig that only apply movements in fixed set of planes at a time. I am thinking a linear  slider connected to a crankshaft. The crankshaft is then connected to stepper motor. On the slider you then have servo with the AHRS mounted on-top. You will then have a linear sinusoidal input, with timed movement on the servo. To test other planes, you can then re-orientate the whole rig, keeping the AHRS horizontal (or not).

[Lew Payne]

Im going to answer both prior posts in this blended post.  My budget for creating a test setup is around $1000 (USD).  A turntable would not satisfy the requirements outlined earlier.  The stepper motor thought prompted some thinking.  An inexpensive hobbyist pan/tilt mechanism (or better yet, p/t/roll) with digital servos (something with as fine resolution as possible) would work.  The mechanism would have to be mounted on the end of something that produces repeatable acceleration forces... preferably not in (or not limited to) a tight (unrealistic - as in turntable sized) circle.

It's important that I be able to test each axis of rotation while a quantifiable (and repeatable) amount of acceleration is applied in a particular plane, since most MEMS double/triple sensors have interaction between the axes (up to 2% or so)... which most tests seem to fail to take into account.  It would appear a stepper would work in this regard.  Take a look at this video for an example of a multi-rate stepper at work.  It's by no means an ideal example, but it's one I've been following in the CNC forums (and have handy).

I might just give up on being the person who does "standardized" testing on these various algorithms (once and for all).

[Lew Payne]

Here's a good paper explaining why the Sigma-Point Kalman filter (aka unscented Kalman filter, central difference Kalman filter) is preferred over the EKF for fusing MEMS sensors...

Sigma-Point Kalman Filters for Integrated Navigation

Here's a PDF slide presentation (by a different research group) on the same subject.  It specifically mentions UAV's, as well as the fact that the filter deals with lagged measurements (inherent to GPS - see this thread) in an elegant manner.

I'm not sure how it performs when compared to the integration method discussed by Sebastian Madgwick (as referenced in an earlier post of mine).  Come to think of it, I don't remember if the Madgwick method includes GPS fusion or if it remains a stand-alone system.  I may be mixing apples and pears.

[dschin]

Hello all,

This is my first post, and I am cynical university professor in engineering going through a mid life crisis. So, with that in mind if you read the following, do so with a bit of skepticism, and try not to hold my cynicism and bad attitude against me too much .

I like the flavor and approach of this project.  But, in my opinion you are wasting time considering all these different variants of optimal state estimation (EKF, UKF, square root implementations, etc.).  Just do a good job on the straight forward EKF and get it working.  Maybe you are falling into the same trap as my graduate students and I do sometimes, where we let the explosion of bad (and some good) info in research articles overwhelm our decisions.  You must realize that the reward systems at universities and the review process for research papers promote the practice of making the benefits of their ideas sound better than they really are and to downplay the draw backs.  Also, the good old solid technique used in industry for years is never as good as the new stuff.  Have you ever read a paper that shows the new, hot topic is worse than the old idea, although we all know that is true in at least SOME cases?

For example, looking at the paper in post below, I see that the real improvement in the simulation results due to the UKF over the EKF (without the latency compensation) is in my opinion not worth the headache you’ll get from doing the UKF, at this stage of your development.  Also, in the real flight data you can see that the results are really pretty much the same except when you have GPS outages, which you hopefully don’t have very often with your good GPS system.

Lewcifer, on 24 May 2010 - 06:13 PM, said:

Here's a good paper explaining why the Sigma-Point Kalman filter (aka unscented Kalman filter, central difference Kalman filter) is preferred over the EKF for fusing MEMS sensors...

Sigma-Point Kalman Filters for Integrated Navigation

Here's a PDF slide presentation (by a different research group) on the same subject.  It specifically mentions UAV's, as well as the fact that the filter deals with lagged measurements (inherent to GPS - see this thread) in an elegant manner.

I'm not sure how it performs when compared to the integration method discussed by Sebastian Madgwick (as referenced in an earlier post of mine).  Come to think of it, I don't remember if the Madgwick method includes GPS fusion or if it remains a stand-alone system.  I may be mixing apples and pears.

It seems you made the right choice a while ago (the EKF) but now are getting (at least in the forum) a little distracted.  I teach an applied course on optimal estimation with a focus on navigation systems.  My experience in simulation (where the truth is known) with the UKF and EKF with the same data is that sometimes one is better other times the other is better.  On actual data from a UAV I find they give pretty much the same answers in as far as being able to tell which is right.  I dunno maybe I just don’t have a good UKF implementation.  The UKF is more computationally expensive, even though it is “derivative free.”  Yes the "Uncented Transformation" usually captures the mean and other statistics more accurately, but I have never seen a proof that the mean, for example, is the most likely value for the true value in a general nonlinear case (not possible to prove) or specifically for any of the real navigation problems (unlikely to prove). Furthermore, in the EKF if you assume the noise covariance matrix for the measurements, R, is diagonal (i.e. uncorrelated noise) then you can implement the needed matrix inversion as a set of  n scalar divisions, where n is the number of measurements.  Uncorrelated noise is probably a pretty good model, and it is unlikely that you will get a good estimate of the cross correlation terms (off diagonal) anyway. The diagonal terms are pretty easy - take some steady data and find the variance of the measurements with everything running for a start. Breaking the calculation into n separate divisions also allows you to easily drop out and add back in available measurements or new types of sensors.  From what I know, which is admittedly not that much, the EKF is still the method of choice by the real practitioners in the navigation industry.  Just as PID is for single-input-single-output control systems if applied right, although it is tuff to find a paper that says fuzzy logic doesn’t work as well as PID.  But you can find a lot of papers that say it does.

Okay, now we do have to realize that the EKF is very well matched to the GPS aided INS problem (GPS/INS), but not as well matched to the AHRS problem.  The covariance matrices, Q and R, respectively for the disturbance noise and the measurement noise, can be used pretty well to model the real situation (this is still true for the UKF also by the way if you go that route).  You have inertial measurements (accels and rate gyros).  These are the "inputs" to the dynamic system model (actually kinematic model), and they have noise that is modeled in Q.  The "measurements" for the model are GPS position and velocity, hopefully magnetometers for heading when hovering, and potentially other sensors, for which we can also model the noise, in R.  Now, Kalman’s proof, which is really just a least squares solution at its heart, shows that the best estimate of the true state is his solution if you have a linear dynamic model and white, Gaussian noise.  It is still the best linear solution to the problem even if the noise is not Gaussian (but white). The EKF is just a linearization of the nonlinear problem.  It is just a good idea that works very well, especially for GPS/INS, not to take anything away from the early giants who came up with all this.  Because the model for the GPS/INS system is purely a kinematic model (not involving the inertias, forces on the moving body, etc.), and the sensor noise is probably pretty much the same no matter what you put it on, you won’t have to change the tunable parameters (Q and R).  It will perform better/worse on different systems, but it is still probably about as good as you are going to get for that platform.

Back to the AHRS – in some of the ARHS algorithms you treat the accelerations of the system due to motion as noise on the accelerometers’ measurement of the gravity vector.  In this case the noise on the inputs (modeled in Q) is dependent how the system moves, which will be different for different systems.  This means the solution might have to be tuned for different systems. ------  I ENCOURAGE YOU TO IMPLEMENT A TRUE GPS/INS ALGORITHM RATHER THAN AN ARHS ALGORITHM AS YOUR BASIC SOLUTION. ----- A descent implementation will be better than an ARHS if you have GPS.  An ARHS algorithm might be needed to keep it flying when you don’t have GPS, although if it just a short loss the GPS/INS can be implemented to carry you through.  You don’t even really need heading to keep it flying.

Now, how to implement?  I suggest a straight forward implementation of the EKF without square root filtering (and the such) just so you can maintain it without scratching your head all the time.  Use the sequential update (n sequential divisions) (described on pg. 151 of “Optimal State Estimation,” Dan Simon, Wiley, 2006, for example) to avoid the matrix inversion.  BTW, the referenced book is a pretty good one for engineers, as apposed to mathematicians, but does not give the details you probably need for your specific implementation. It is actually pretty hard to find a good, to the point, description of the full model etc… although I think I have one for GPS/INS without mags if one of the main developers contacts me.  Mags would be easy to add.  I also could probably dig up an old, mostly clean, C implementation from a few years back, used in a crude autopilot I did (Currently, I just use the Cloud Cap solution in Piccolo for the Nav solution and modify the control code for low level control and guidance loops.)  I could be wrong but I think your AHRS processor could do all this without too much problem (does it have access to the GPS?).  You are looking at a “loosely coupled” solution I am pretty sure, where you use the GPS position and velocity solutions as measurements rather than the raw pseudo range measurements (that involves satellite ephemeras data also – would be pretty crazy and overkill).

Anyway I have said waaaaay too much and you probably won’t read it given my curmudgeon attitude .  I do welcome discussions with one of the main developers if you so desire.

[PeterG]

Yes, with a post that long you are not really helping us with our time wasting.

Seriously though, welcome and thanks for chiming in! The reason that I originally asked about the UKF was that I don't trust papers (nor professors necessarily) and I was hoping that someone would have real experience with it. The aerospace industry can be a bit conservative at times, so that alone would not be proof to me that EKF is just as good or better.

I think everybody wants OpenPilot to have the best possible AHRS/INS, given our limited hardware, limited time and limited intelligence. In my opinion, trying to make an informed decision is not a waste of time, especially not as the AHRS hardware is not yet available. Besides, OpenPilot is a project that a lot of us enjoy wasting our time on.  

Anyway, it makes sense to me to start with an algorithm that we can be reasonably sure that it will be correctly implemented, and if that means "the straight forward EKF" I'm all for it. Of course, this doesn't prevent anyone from implementing UKF or any other algorithm if they want to, the source code is open. To me it all boils down to 1. what interests, skills and knowledge the person(s) actually writing code has, 2. what knowledge other people are willing to share and 3. what information you can extract from papers and books.

Peter

[Lew Payne]

dschin - From one mid-life crisis person to another, welcome!

As far as your comment on the flavor and approach of OpenPilot, I've reread the mission statement and still conclude that its goal is to build one of the best open pilots out there that rivals others.  In particular, it talks about not compromising on things such as component choice and design.  By the same token, that leads me to believe it would rather not compromise on algorithms either.

Advocating the use of a straightforward EKF by simply dismissing current research as theoretical or flawed simply amounts to ignoring years or progress that has been made since "the old days" you and I thrived in.  I try and stay current with modern techniques, as a way of staying mentally young.  For the same reason, I watch South Park, dewd!

What, specifically, is the headache we'll get in "doing the UKF?"  Comparatively speaking, how does that particular "headache" differ from the one we'll get by doing the EKF?  Note that there's plenty of source code available for many flavors of Kalman filters, especially in MatLab (as well as several C++ open-source libraries).

In some of the papers I cite, real flight data shows a marked increase in accuracy using SPKF (UKF) for fusion, even w/o GPS outages.

When you say the UKF is more computationally expensive, the statement doesn't really convey any relevant data.  Is the UKF so computationally expensive that the given hardware platform is unable to implement it effectively?

Uncorrelated noise will be present in this model.  You can't help but swallow some UCN, given the nature of the MEMS sensors.  True, it can be reduced by implementing an oversampling model that decimates the data, but that won't account for all UCN and places an extra burden on the ADC, although in all fairness the STM32 has some features that make oversampling less painful.

There's also the issue of EKF tuning.  A UKF tends to converge faster even with poorly chosen tuning parameters.  In some extreme cases, the EKF may never converge whereas the UKF will.

I have several of Dan Simon's books and publications. He's spent a few decades immersed in Kalman and other filters (IIR, FIR, particle, etc) and is certainly well studied in those fields.  He has not, however, had to address a situation in which GPS latency skews the measurements.  Because terms can mean different things to different people, let me define the term, explicitly.

GPS latency, as used in my posts: The effect of trying to fuse a GPS reading with other IMU/AHRS/MARG sensor data on a vehicle capable of travelling 100 mph (FunJet) in an arc (tight turn).  Due to the nature of the beast, GPS units lag... they output filtered data, and need to "average" multiple readings to obtain a vector.  As such, depending on GPS filter implementation, a 10Hz GPS can lag anywhere from 3 readings (300mS) to as much as two (2) seconds.  At 300mS GPS lag, the latency amounts to 43 feet and the vector no longer resembles the vehicle path during tight turns.  At 2000mS GPS lag, the latency amounts to 293 feet!  How you propose to fuse that back into a more accurate reading, without compensating for the reality of GPS lag, is beyond me.

As a kid, I always thought KF stood for "Kentucky Fried" and the "C" part was just a given.  But that's beside the point.

[CheBuzz]

I think it may be beneficial to do some statistical analysis on the OP GPS units, as they are running a custom firmware made specifically for the project.  It would be nice to pin down precisely how much lag to expect, as well as performance in a turn, how many satellites it will hold, etc.

[dschin]

Lewcifer

Lewcifer, on 27 May 2010 - 06:45 PM, said:

As far as your comment on the flavor and approach of OpenPilot, I've reread the mission statement and still conclude that its goal is to build one of the best open pilots out there that rivals others.  In particular, it talks about not compromising on things such as component choice and design.  By the same token, that leads me to believe it would rather not compromise on algorithms either.

Your right.  I'm just excited to see the first, solid working version.  Then start making improvements.  I think just having true INS/GPS algorithm rather than just an AHRS will be a huge improvement over alot of what's out there open source.

Lewcifer, on 27 May 2010 - 06:45 PM, said:

What, specifically, is the headache we'll get in "doing the UKF?"  Comparatively speaking, how does that particular "headache" differ from the one we'll get by doing the EKF?  Note that there's plenty of source code available for many flavors of Kalman filters, especially in MatLab (as well as several C++ open-source libraries).

In the course that I teach on this I find that implementing the UKF rather than the EKF is more prone to errors and computationally expensive.  My students make alot more mistakes implementing the UKF, even when they are trying to apply available code to a new problem.  Personally I find it harder to wrap myself around.  Also there a few really good ways to reduce the computations for the KF equations that I haven't seen for the UKF.  But, I admittedly haven't looked that hard.

I don't want to discourage people. I just want to see a first good, solid system.  Don't let me be a downer.

[Lew Payne]

dschin, on 28 May 2010 - 07:27 PM, said:

Your right.  I'm just excited to see the first, solid working version.  Then start making improvements.  I think just having true INS/GPS algorithm rather than just an AHRS will be a huge improvement over alot of what's out there open source.

I agree with this 100.00001% (sorry... floating point rounding error).  It is painful and costly to change hardware after the fact... but software can continually be improved (providing the hardware was designed with all the "hooks" necessary).  As such, it is indeed a good idea to crank out a solid working version, and then let those who like to toy with software (such as myself) bang their heads on the wall improving it.

Can't argue with that!

[Lew Payne]

CheBuzz, on 28 May 2010 - 08:37 AM, said:

I think it may be beneficial to do some statistical analysis on the OP GPS units, as they are running a custom firmware made specifically for the project.  It would be nice to pin down precisely how much lag to expect, as well as performance in a turn, how many satellites it will hold, etc.

If the OpenPilot group did get the [GPS chip] manufacturer to install a different algorithm that is better suited to higher speeds, I would love to know the specs on this and what they were able to do.  In fact, I'd imagine it's important for the entire OpenPilot community to know what was done, and how, so that the project is not subject to "key man" issues (and so someone working on fusion knows what latency to expect).

[cwabbott]

Hi everybody,

I've been reading all sorts of things about AHRS and INS units, because I think it's an interesting problem, and I have a couple of questions. Keep in mind that I'm one of those people who like to do this to waste my time , and as such have zero experience except for what I've read online.

1) I know that Kalman filters have one or more "predict" phases and then an "update" phase. I'm assuming here the "update" phase will only occur when new GPS data is available to the AHRS/INS. Am I right?

2) Dschin has said that the measurements for the update phase are GPS position, velocity, and the mag sensor. How it it possible to correct for drift in pitch and roll using only these sensors? I thought you estimated linear accel using the GPS velocity measurements (to provide an independent source, besides the estimation of linear accel via the accelerometers) , subtracted it out of the accelerometers, and then use that to determine pitch and roll. It may be really noisy, but that's the only way I imagine you can get an approximation of pitch and roll without the problem of linear accelerations screwing up the accel data during a turn. Is there something I'm missing?

[murmur]

cwabbott, on 05 June 2010 - 08:16 PM, said:

Hi everybody,

I've been reading all sorts of things about AHRS and INS units, because I think it's an interesting problem, and I have a couple of questions. Keep in mind that I'm one of those people who like to do this to waste my time , and as such have zero experience except for what I've read online.

Hi cwabbot, I'm not too far ahead of you but I'm learning. Here's what I think I know regarding your questions:

cwabbott, on 05 June 2010 - 08:16 PM, said:

1) I know that Kalman filters have one or more "predict" phases and then an "update" phase. I'm assuming here the "update" phase will only occur when new GPS data is available to the AHRS/INS. Am I right?

That's right. The filter implementation computes the state and covariance histories at small time steps, between the larger 1Hz intervals when you get GPS updates. It computes those state and covariance histories based on the high-rate outputs of the inertial (gyro and accel) and magnetic sensors (and baro altitude if it's available). The filter only really cares what the covariance matrix has in it at the 1Hz GPS update times, but it needs to compute that matrix at smaller time steps in order to get the right answer at the 1Hz points. On the other hand, the state vector contains useful information for the autopilot to use in controlling the vehicle at the higher-rate steps (i.e. pitch, roll, and yaw in the AHRS case, those plus three-dimensional positions and velocities from an INS)

cwabbott, on 05 June 2010 - 08:16 PM, said:

2) Dschin has said that the measurements for the update phase are GPS position, velocity, and the mag sensor. How it it possible to correct for drift in pitch and roll using only these sensors? I thought you estimated linear accel using the GPS velocity measurements (to provide an independent source, besides the estimation of linear accel via the accelerometers) , subtracted it out of the accelerometers, and then use that to determine pitch and roll. It may be really noisy, but that's the only way I imagine you can get an approximation of pitch and roll without the problem of linear accelerations screwing up the accel data during a turn. Is there something I'm missing?

I don't think that's quite right. GPS data is used to correct the estimates of the error states, and by error states I mean the numbers that describe errors in the instrumentation (gyros, accels, possibly the magnetometer). Those instrument-error estimates are then used during the high-rate computations of roll, pitch and yaw (and velocities and positions if applicable) to correct the errors in the instrument outputs that go into those computations. So it isn't just at GPS-update times that "corrected" estimates of pitch and yaw are computed; it's at every high-rate time step. Via the "magic" of the Kalman filter, best estimates of the corrections are arrived at over time, and depend (among other things) on the fact that the system equations in the filter contain a description of how errors in the instrumentation (like biases and scale factors) end up causing errors in other states like roll, pitch, yaw, and the three-dimensional velocity and position.

[Lew Payne]

Keep in mind that the inclination of the Earth's magnetic field, in most places (except equatorial), is roughly a 60 degree incline (rather than a horizontal influence)!  As a result, there is plenty of magnetic field to measure movements in directions that you'd intuitively think there's no flux in.

[cwabbott]

murmur, on 11 June 2010 - 03:14 AM, said:

Hi cwabbot, I'm not too far ahead of you but I'm learning. Here's what I think I know regarding your questions:


That's right. The filter implementation computes the state and covariance histories at small time steps, between the larger 1Hz intervals when you get GPS updates. It computes those state and covariance histories based on the high-rate outputs of the inertial (gyro and accel) and magnetic sensors (and baro altitude if it's available). The filter only really cares what the covariance matrix has in it at the 1Hz GPS update times, but it needs to compute that matrix at smaller time steps in order to get the right answer at the 1Hz points. On the other hand, the state vector contains useful information for the autopilot to use in controlling the vehicle at the higher-rate steps (i.e. pitch, roll, and yaw in the AHRS case, those plus three-dimensional positions and velocities from an INS)


I don't think that's quite right. GPS data is used to correct the estimates of the error states, and by error states I mean the numbers that describe errors in the instrumentation (gyros, accels, possibly the magnetometer). Those instrument-error estimates are then used during the high-rate computations of roll, pitch and yaw (and velocities and positions if applicable) to correct the errors in the instrument outputs that go into those computations. So it isn't just at GPS-update times that "corrected" estimates of pitch and yaw are computed; it's at every high-rate time step. Via the "magic" of the Kalman filter, best estimates of the corrections are arrived at over time, and depend (among other things) on the fact that the system equations in the filter contain a description of how errors in the instrumentation (like biases and scale factors) end up causing errors in other states like roll, pitch, yaw, and the three-dimensional velocity and position.

Okay, so I read a lot about Kalman filters (specifically EKF) over the last couple days, and now I know a little more. Let me summarize here what I understand about the EKF:

When using an EKF, you need to provide two functions mapping the IMU sensors' values to the state vector, x:
-the state update equation (or nonlinear stochastic difference equation - say that 5x fast) f, tells you an "a priori" estimate of the state at k + 1 (for our purposes, we use the gyro and accel as "control inputs" and use them to update the state)
-the measurement equation h, tells you the value of the measurements (accel - the estimated linear acceleration is added somehow to the estimated gravity vector, mag, GPS) given a certain state x
Using these and their respective covariance matrices Q and R, the filter then operates as follows:
-the state update equation is linearized about the current state
-the state is actually updated
-the jacobians computed previously are used to estimate the increase in the estimate's covariance P (due to drift) given Q.
-the first three steps can be repeated indefinately until a GPS measurement is given
-now, the jacobians can be used to compare the actual measurements with the estimated ones (via the measurement equation), and estimate the best fit linearly using the Kalman gains (i.e. "magic"), as well as estimate the reduction of P due to the "magic" that keeps the covariance in check.

This is mostly correct, right? I may have gotten the order of some parts wrong.

Anyways, I think that what I meant by "correction" was confused. By "error states," you mean the offset in data from gyro drift, linear acceleration, etc., right? Exactly how does this "error vector" fit into the Kalman equations for state predict and update? Is it just another part of the state vector? I've heard of this before, but curiously enough I've never seen such an "error state/vector" variable in the actual equations (like here). Is it just the covariance (I thought this would only represent zero-mean gaussian noise, but I really have no clue ...)?

The quest continues...

As for GPS/INS vs. AHRS , I think that you should adopt an as flexible interface as possible, allowing for both types of designs to coexist in the build-tree (and maybe even the final firmware, to allow for run-time switching between filters, although this would be very difficult) so that the end-user can choose which is more appropriate to their needs. The main board would ping the AHRS at startup, asking what the filter setup is and then setting up modules appropriately. I would implement three modes:

1) AHRS - the AHRS recieves PositionActual data but does not send anything back except orientation.
2)GPS/INS - the AHRS will send out position data either once it recieves GPS data in or at 512 Hz (choose one - I would go for the latter because you don't really need 512 Hz position data).
3)GPS/INS w/ state model - if you can provide an estimate of the acceleration based on the servo positions and/or the current state (like one of the papers referenced in the wiki - The True Role of Accelerometer Feedback in Quadrotor Control), then use this. The main board will send the current servo positions to the AHRS, and this will fuel the state prediction of position instead of the accelerometer.

Thanks for reading my long rant,
Connor

[cwabbott]

On a slightly unrelated note, I got the newest Discovery magazine yesterday and I have to quote from pg. 57:

Quote

Bad measurements are not even the biggest source of wrongness  in scientific studies. That honor would have to go to "publication bias" - journal's tendency to eagerly publish the small percentage of studies that produce exciting, surprising, breakthrough results. Of course, the most likely explanation for why one team of reaserchers comes up with surprising results while several other teams get less-pulishable, boring results is that the one team screwed up somewhere. The pernicious effect of this phenomenon on the trustworthiness of study results has been documented at length in scientific journals themselves in several fields.

[murmur]

cwabbott, on 12 June 2010 - 03:31 AM, said:

Okay, so I read a lot about Kalman filters (specifically EKF) over the last couple days, and now I know a little more. Let me summarize here what I understand about the EKF:

*snip*

This is mostly correct, right? I may have gotten the order of some parts wrong.

Yes, I think that's all pretty much correct.

cwabbott, on 12 June 2010 - 03:31 AM, said:

Anyways, I think that what I meant by "correction" was confused. By "error states," you mean the offset in data from gyro drift, linear acceleration, etc., right? Exactly how does this "error vector" fit into the Kalman equations for state predict and update? Is it just another part of the state vector? I've heard of this before, but curiously enough I've never seen such an "error state/vector" variable in the actual equations (like here). Is it just the covariance (I thought this would only represent zero-mean gaussian noise, but I really have no clue ...)?

I think I caused (myself and you) some confusion by referring to "error states". What I meant by "error states" is the subset of the state vector that's there to estimate instrument biases and scale factors (if the filter designer has chosen to have the filter do such estimation). So yes, I was using it to refer to just another part of the state vector, but in reality the "error vector" commonly means something else in the literature: the difference between the true state vector and the estimated state vector (which is something you never actually have in the implementation; it's just an interim quantity that appears in derivations.). So it's better to not call those bias- and scale-factor-estimation states anything other than just that.

So anyway, these states that are put in the system model to capture instrumentation errors and the like have their dynamics determined by, not surprisingly, what appears in the A matrix. And, other A-matrix entries describe how these instrumentation errors affect the other states, like estimated angular position and velocity, etc. So the system designer will put dynamics in that match what he thinks the real behavior of the biases and scale factors are, things like is a bias really a bias or is it just a very slowly-varying random walk, etc. At the same time, the designer can put values in the Q (process noise) matrix to help the covariance matrix track error sources for which the dynamics aren't really known (precluding their modeling as a state), hopefully making the results of updates more accurate.

[cwabbott]

Murmur,

I think your response cleared up a lot. Still, I have a few more questions about the way the error states are propagated through the filter. Here's the way I see it:

1) Gyro bias: The state model assumes it's constant and puts the change in bias due to temperature into the error vector (random walk), and then it's transfered directly to a measurement by the measurement function. It is measured by estimating the difference between the predicted and measured orientation and dividing by time. Of course, you could also factor in changes of the temp probe in determining how it will change in the state model.
2) Linear acceleration: Same deal as gyro bias, except it's measured via the difference between GPS velocity and the velocity some point in time back (could be anywhere, there are tradeoffs of higher noise vs. smoothed acceleration).
3) Magnetometer hard-iron and soft-iron errors: Again, same thing, except I'm not sure how it's estimated. Could use least-squares estimation or something similar...

What do mean by "dynamics determined by ... the A-matrix"? You mean the Jacobian one right? I'm sure there's some explanation for these error states/biases in your post that I'm missing.

[murmur]

cwabbott, on 17 June 2010 - 01:18 AM, said:

Murmur,

I think your response cleared up a lot. Still, I have a few more questions about the way the error states are propagated through the filter. Here's the way I see it:

1) Gyro bias: The state model assumes it's constant and puts the change in bias due to temperature into the error vector (random walk), and then it's transfered directly to a measurement by the measurement function. It is measured by estimating the difference between the predicted and measured orientation and dividing by time. Of course, you could also factor in changes of the temp probe in determining how it will change in the state model.
2) Linear acceleration: Same deal as gyro bias, except it's measured via the difference between GPS velocity and the velocity some point in time back (could be anywhere, there are tradeoffs of higher noise vs. smoothed acceleration).
3) Magnetometer hard-iron and soft-iron errors: Again, same thing, except I'm not sure how it's estimated. Could use least-squares estimation or something similar...

No, the state model doesn't have to assume (things like) gyro biases are constant if you define a state for each bias (i.e. the "error vector" is just a subset of the state vector). So the biases are estimated right along with everything else in the state vector, with the characteristics of the resulting bias estimates (like time constants and mean-square value) controlled by the combination of the modeling for them in the state equations AND the entries along the diagonal of the Q matrix that define their statistics.

Your statements relating updates to the instrument-bias estimates (don't forget instrument scale factors which are commonly estimated in much the same manner, by the way) to the measurements is correct in a heuristic sense, although the actual update equations would ideally include contributions from all three of attitude, position, and velocity measurements in the updates to gyro errors. This is the kind of thing I'm going to be arguing for to be included in the AHRS code framework, since a more complete formulation of the predictor-corrector approach will be easier to adapt to new applications.

cwabbott, on 17 June 2010 - 01:18 AM, said:

What do mean by "dynamics determined by ... the A-matrix"? You mean the Jacobian one right? I'm sure there's some explanation for these error states/biases in your post that I'm missing.

That's right, in an EKF the A matrix is made up of the partial derivatives of the nonlinear 'f' and 'h' functions that define the system. Here are a couple of partial pages from Welch and Bishop's "Intro to the Kalman FIlter" paper from 2006:

 EKF2_Welch2006.jpg   93.17K   107 downloads
 EKF_Welch2006.jpg   93.38K   92 downloads

where the A matrix (and the noise and measurement-sensitivity matrices, too) are defined as arrays of partial derivatives of the original nonlinear functions 'f' and 'h' (the state-equation and measurement-sensitivity equation, respectively).