25 replies to this topic

[Jonathan Brandmeyer]

I am working on cleaning up some research-oriented KF code for field use. There's just one big catch: It's all in C++. I'm using the Eigen C++ library for all of the linear algebra. I've avoided dynamic memory allocations, EH and RTTI since those features aren't really appropriate for realtime systems. Nonetheless, the compiled code is big. The code is written in a way that depends strongly on the compiler for its performance, although a number of algorithmic optimizations have been performed already. At -O2, it tips the scales at about 70 kB of x86 .text and at -O3 it weighs in at over 120 kB in the .text segment (the .data segment is only about 1.5 kB in both cases).  Stack utilization is also going to be high (probably several kB), since all of the temporaries are allocated on the stack. Nevertheless, I consider it appropriate to use the stack as a workspace since the same workspace can be trivially used by both the prediction and update steps.

The filter is a multiplicative 12-state kinematic GPS-INS EKF. It supports vector observations and GPS position and velocity reports in ECEF or NED coords.  I'm a little bit worried about the ability of the filter to be crammed into an STM32, mostly for speed reasons. If I force my old Pentium-M laptop down to a 600 MHz, then I get the following hot-cache timings:

• prediction (gyro and accelerometer measurement): 10 us
  
• vector measurement (magnetometer): 18 us
  
• GPS PVT: 24 us

Oddly enough, the GPS PVT runs slower when the position update is performed using rank-one updates, at about 32 us. But on x86-64, using rank-one updates for position is marginally faster.  Using rank-one updates in velocity

The filter's ability to reliably converge from an initially unknown state is somewhat strongly dependent on the rate and quality of GPS velocity reports. The velocity report contains a great deal of orientation information in the cross-covariance between the velocity block and the orientation block.  4Hz is barely OK, 10Hz is much better.  Likewise, the nonlinearity in the state update (particularly in the relationship between acceleration and orientation) also demands a fast update rate. 100 Hz is barely passable, 250Hz is much better.  Likewise for magnetometer readings, 50Hz is good, 25Hz is marginal, and 20 Hz runs a risk of filter divergence.

So, if we assume that the STM32 can get about 1 MFLOP of dp throughput, and that my P-M is producing about 600 MFLOPs (really rough guessing here), then we estimate the following STM32 timings:

• 14 ms for GPS PVT update @ 10 Hz = 140 ms per second
  
• 6 ms for state prediction @ 100 Hz = 600 ms per second
  
• 11 ms for vector observation @ 25 Hz = 220 ms per second

That's close to 100% CPU utilization. Now, the aforementioned assumptions are really rough, so that could easily be anywhere from 50-150%, but its close enough for some discomfort. (or close enough to be excited if you're a glass-half-full kindof guy).

By "barely passable" I mean that under 5% of all initial orientations have a state estimate that is greater than 10 sigma away from the true state after six seconds of filtering, with all monte carlo test runs converging and none diverging.  Much better is only 2% greater than 10 sigma away, with most less than 4 sigma.

Initial conditions are:

• Random initial orientation
  
• Random magnetic field orientation
  
• Gaussian random initial bias, with one sigma bounds of 5 deg/s
  
• Initial random velocity up to pi rad/s in a randomly chosen rotation axis
  
• Initial random angular vibration up to pi/2 rad/s at freq chosen over log domain of 10 Hz to 1/20 Hz at another random direction
  
• Initial random velocity up to 30 m/s in random direction
  
• Initial random acceleration up to 0.5 G in random direction

Worst-case convergence behavior is observed when the acceleration vector is collinear with the magnetic field.  Even under the "barely passable" situations, the filter will always converge to the right orientation as long as the total acceleration vector is not colinear with the earth's magnetic field. After initial convergence, they have to be collinear for a while before the filter drifts too far off.

I hope to open up a bitbucket account this weekend to host the OS code. I think I'm going to have to handle this code very carefully for two reasons:

• The USA has tight export restrictions on this sort of technology, and I must comply with my govt's laws. I'm drawing from the experience of crypto researchers in the late 90's to come to this conclusion: Its one thing to publish generic source code for an algorithm. It's another thing entirely to directly contribute to a complete working autopilot that is principally hosted in another nation, even if that nation is considered a friend.
  
• In the Great State of NC, what I do on my own time with my own resources is my IP. I am fully employed as an embedded software engineer in an unrelated industry. However, we are also using FreeRTOS on an STM32, so I have to tread carefully here.  Contributing to PiOS might be just a little too gray for me, even though it is something that I would like to do.

Therefore, I think that the most appropriate way for me to make this work available to the community is to host it is in the form of an external library. I'll do my best to ensure that the library is written in a way that encourages third-party linkage rather than copy-and-paste, but I don't think that I'll be able to directly submit patches to an autopilot project outside of the US.  Besides, from a project management perspective, this makes good sense, since it means that any FOSS autopilot would be able to use the same interface if they so chose.  The license with be GPL v3 and no other version.  BSD and LGPL are out of the question.

[Jonathan Brandmeyer]

Jonathan Brandmeyer, on 26 June 2010 - 04:30 PM, said:

I hope to open up a bitbucket account this weekend to host the OS code.

And it's up. The build system is downright primitive. You'll have to edit the Makefile manually based on your current setup, but this should be enough to start discussing its use in openpilot.

[Jonathan Brandmeyer]

Jonathan Brandmeyer, on 27 June 2010 - 08:08 PM, said:

Using rank-one updates in velocity

Let me complete the statement. Using rank-one updates in velocity runs a risk of filter divergence, because it introduces a large "factorization nonlinearity" when updating the quaternion-valued orientation mean.  Factorization nonlinearity is the nonlinearity that arises from considering rotations in 3D to be linearly factorizable into some set of basis vectors.  The cross-covariance between velocity and attitude is strong during filter operation, but the cross-covariance between position and attitude is weak. Therefore, you can get away with rank-one updates in position, but not in velocity.

[Lew Payne]

As Yi Cao (links to MatLab code) would say...

"Nonlinear state estimation is a challenge problem. The well-known Kalman Filter is only suitable for linear systems. The Extended Kalman Filter (EKF) has become a standarded formulation for nonlinear state estimation. However, it may cause significant error for highly nonlinear systems because of the propagation of uncertainty through the nonlinear system.

The Unscented Kalman Filter (UKF) is a novel development in the field. The idea is to produce several sampling points (Sigma points) around the current state estimate based on its covariance. Then, propagating these points through the nonlinear map to get more accurate estimation of the mean and covariance of the mapping results. In this way, it avoids the need to calculate the Jacobian, hence incurs only the similar computation load as the EKF."

For the purpose of implementing a UKF on STM32 (no hardware floating point), I suggest looking at the Sigma-Point Kalman filter, of the square root flavor.  See here and here for additional rhetoric by yours truly.  It's important to keep in mind GPS latency (from the GPS internal Kalman or similar processing, resulting in a lagging average... especially hideous during turns) when doing so. See here for an overview slide presentation.

[Jonathan Brandmeyer]

Lew Payne, on 28 June 2010 - 09:34 PM, said:

As Yi Cao (links to MatLab code) would say...

In this way, it avoids the need to calculate the Jacobian, hence incurs only the similar computation load as the EKF

When I started implementing my EKF last fall, I also implemented a UKF state propagation algorithm as well. My finding was that the UKF propagation method consumed 5x additional CPU time, with no improvement whatsoever in initialization or tracking performance.  Crassidis and Markley found in "Unscented Filtering for Spacecraft Attitude Estimation", that a highly optimized AHRS UKF consumed 2.5x additional CPU time with no improvement in tracking performance (but significant improvements in initialization performance).  So the claim that the UKF consumes the same runtime as the EKF is flat wrong. It may have the same asymptotic order of complexity as the number of states increases (O(n³)), but that analysis does not include the significant constant factors that are involved.  Also, the calculation of the Jacobian for this navigational EKF consumes a very small amount of runtime compared with the multiplication A*P*A^T itself.

[Lew Payne]

Jonathan Brandmeyer, on 29 June 2010 - 01:49 AM, said:

My finding was that the UKF propagation method consumed 5x additional CPU time, with no improvement whatsoever in initialization or tracking performance.  Crassidis and Markley found in "Unscented Filtering for Spacecraft Attitude Estimation", that a highly optimized AHRS UKF consumed 2.5x additional CPU time with no improvement in tracking performance (but significant improvements in initialization performance).  So the claim that the UKF consumes the same runtime as the EKF is flat wrong. It may have the same asymptotic order of complexity as the number of states increases (O(n³)), but that analysis does not include the significant constant factors that are involved.  Also, the calculation of the Jacobian for this navigational EKF consumes a very small amount of runtime compared with the multiplication A*P*A^T itself.

It seems to depend on whose paper you read, how their SPKF implementation is coded, the programming language its coded in, the target machine and compiler used, the compiler optimizations, and (as another possibility) if it was a purely theoretical exercise (i.e., simulation) or a real-life implementation.  Using Rudolph van der Merwe's implementation as a reference, the SPKF had the same computational load as the EKF, gave a more accurate account of nonlinerarities, and provided exact modeling of asynchronous and lagged sensor updates.  Here's a PDF slide presentation (summary).

Rather than theoreticals, Dr. van der Merwe tested his SPKF against the Cloud Cap Crista IMU (~$2K) and Inertial Sciences ISIS-IMU (~$10K).  The results (on page 14) speak for themselves (SPKF with GPS latency compensation provided the lowest average RMS error).  Did your UKF account for GPS latency?  If not, we're talking apples to oranges here. Traditional Kalman approaches don't seem to take that into account, especially when using consumer-grade GPS, with no point solution output or latency statistics output (for use by a compensation algorithm).

I'm in contact with Dr. van der Merwe, trying to see if he'll release source code.  He's also contributed some MatLab code, in the ReBEL toolkit.

[pfg]

Very interesting - I must look at this code. Tell me tho, is it amenable to conversion to fixed point ? floating point is truly unnecessary when those state's are being driven by 12 bit a/d values. One could make a case for float's using all 24 bit a/d's but the device's themselves have specs no were near that

[Haldir]

pfg, on 29 June 2010 - 09:33 AM, said:

Very interesting - I must look at this code. Tell me tho, is it amenable to conversion to fixed point ? floating point is truly unnecessary when those state's are being driven by 12 bit a/d values. One could make a case for float's using all 24 bit a/d's but the device's themselves have specs no were near that

I don't know of any sr-ukf (square-root unscented kalman filter) written with fixed-point arithmetics.

If we consider a larger state sr-ukf, with 16 to 19 states (Dr. Merwe's model was 16 states, if you add 3 bias for magnetometers we're at 19) you won't run the sr-ukf on any small mpu with a decent update rate of > 100hz.
As for Helicopters/Quadrocopters we need more than 100hz (atleast according to the several papers on that topic) for a decently reacting system.

As for C++ sr-ukf I currently just have David Sachs' code, with 7 states and an update rate of 1kHz on a Core 2 Duo E8500. Even though that code is not the special case "additional noise", I don't think we can get to 16-20 states with 500Hz on anything short of a Cortex A9.

As soon as I've finished my own sr-ukf code in Matlab with (currently projected 20 states, (19 incl. magnetometer and one for barometer bias)), I'll do some C++ implementation tests.

Anyway I think that the sr-ukf is the right way to go, including gps-latency correction (if the cpu permits) and later on switching from a loosely-coupled INS with GPS to a tightly and later maybe a ultra-tightly coupled system as the mcpu power increases.

[Jonathan Brandmeyer]

Haldir, on 29 June 2010 - 12:10 PM, said:

I'm in contact with Dr. van der Merwe, trying to see if he'll release source code. He's also contributed some MatLab code, in the ReBEL toolkit.

Competition is most welcome in this space! But beware, that their platform was a 300 MHz DSP (a "DSP400", whatever that means).

My next step will be to write a program that takes a CSV file of sensor readings as input, and produces a CSV file of estimated states as its output. That way we can perform head-to-head tests between different filter implementations.  Almost as valuable as his program source code would be the data streams that are backing the graphs in his papers. Don't be too demanding of the file format, we can massage the data into some common standard ourselves as long as the data is readable using open tools.

[Lew Payne]

Haldir, on 29 June 2010 - 12:10 PM, said:

If we consider a larger state sr-ukf, with 16 to 19 states (Dr. Merwe's model was 16 states, if you add 3 bias for magnetometers we're at 19) you won't run the sr-ukf on any small mpu with a decent update rate of > 100hz.

My primary development system is the TI TMS320C28346 (Delfino) DSP MCU with enhanced floating point (for another NAV project).  I will then see what it takes to port it over to STM32 (of which I also have a full development system).  However, there's something else (algorithm-wise) which is catching my eye, discussed below.

Jonathan Brandmeyer, on 29 June 2010 - 12:49 PM, said:

Competition is most welcome in this space! But beware, that their platform was a 300 MHz DSP (a "DSP400", whatever that means). [File dump of sensor data used for the graph].

Well, my primary platform is a 600 MFLOPS DSP.  Since this particular SPKF can handle asynchronous lagged data, it may be possible to "tweak" the individual sensor update rates such that it works on an STM32-based platform.  That thought, in and of itself, also opens up a whole set of other curiosities I might enjoy exploring, such as... "must the update rate be the same for XYZ?"  If one of the dynamic components tends to change less frequently than the others, it might be a candidate for slower updates and lagged integration.

The other thing which catches my eye is the work of Sebastian Madgwick (see this post).  His algorithm is suitable for use on an abacus.  I'd like to figure out a way of incorporating GPS latency compensation into it.  I'm thinking it might actually require modeling the GPS latency; something like going through 90 degree turns at an "average" speed and seeing how far off the GPS is (before its algorithm puts the lat/lon back on my vector).  From there, a general latency formula (specific to that GPS chip) can be developed to inject compensation based on sensor input (a change in direction).  That's the final step, which others (researchers) have not yet done.

One thing that some researchers are doing is weighting the Kalman filter and integration model, depending on motion conditions: steady-state or not.  I don't remember if Madgwick's model also does this (have read too many papers) - it might.  This allows for more accurate sensor data over a range of dynamic conditions (gyros when their output is best utilizied, and the same for accelerometers).

[Haldir]

Lew Payne, on 29 June 2010 - 05:16 PM, said:

My primary development system is the TI TMS320C28346 (Delfino) DSP MCU with enhanced floating point (for another NAV project).  I will then see what it takes to port it over to STM32 (of which I also have a full development system).  However, there's something else (algorithm-wise) which is catching my eye, discussed below.

Funny, my primary development system is the TI TMS320F28335, so I guess we'll have comparable results sooner or later.

Anyway, as for another comparison of SR-UKF with EKF, here is another, not so well known, paper about the topic: http://www.iasi.cnr....R8008/R8008.pdf

The main difference to Dr. Merwe's work is that it includes a magnetometer in the process model. My goal is using their process model to get an idea about the necessary computing power to run a 19 state sr-ukf (with additive noise). The current target is 100Hz on the TI 300Mhz Delfino.

[Lew Payne]

Haldir, on 29 June 2010 - 05:58 PM, said:

Funny, my primary development system is the TI TMS320F28335, so I guess we'll have comparable results sooner or later.

Anyway, as for another comparison of SR-UKF with EKF, here is another, not so well known, paper about the topic: http://www.iasi.cnr....R8008/R8008.pdf

The main difference to Dr. Merwe's work is that it includes a magnetometer in the process model. My goal is using their process model to get an idea about the necessary computing power to run a 19 state sr-ukf (with additive noise). The current target is 100Hz on the TI 300Mhz Delfino.

Between the STM32F and TMS320F, I can no longer keep their suffixes memorized.  In composing this reply, I searched for my other "TI DSP" post and found that I had screwed it up there, also.  How embarrassing.  Yes, it is in fact the TMS320F28335 and the STM32F103 (almost wrote it as STM320F just now).  So it seems we're both using the same TI DSP (as target), except my development board is a lesser TI chip... so I can't actually benchmark floating point ops with it until ported to the target.

Thanks for the paper... it's one I have not yet run into.  I am slowly going through the research I've gathered on this subject, and putting it all in one place - while commenting on the specific section that interests me.  You can see some of that work in my technical blog, which is simply a place for me to keep this stuff somewhat organized.  All relevant papers are not yet in there, as I first want to comment on all the existing ones before adding more.  Help yourself.

If there's any collaboration or head-scratching you'd like to do together, please... by all means... let me know.

[Lew Payne]

Haldir - Thanks so much for the new technical paper by Fiorenzani, et-al.  Two noteworthy comment from the paper, which I know I've stated somewhere else in this forum at some point in time (my memory is old and worn out)...

Quote

The simulations showed that UKF and EKF, independently from the loss of GPS (for 40 second intervals), give very similar results while having a small initial estimation error. We now consider the case of a large initial error.  In the following simulation, the initial attitude error is of about 40 degrees on each angle, and also the initial state covariance is larger.  In this case, the UKF has a faster convergence [emphasis added] with respect to the EKF, but after a settling time the performance becomes identical.

Quote

An interesting case is the one which sees a wrong setting of initial covariance.  If the initial estimation error is very large, but the initial covariance is not increased according to it, we have the results showed [sic] in figure 16.  The EKF diverges while the UKF converges [emphasis added]. The divergence of attitude angles brings also the divergence of all the other state variables.

Both of these are noteworthy to me because faster convergence is particularly important during takeoff and, more importantly, an improperly tuned EKF can diverge and crash the UAV.  While there are technical papers on how to tune an EKF (i.e., how to compute "optimal" initial values), the dynamics involved are beyond the scope and ability of most hobbyists (or lazy people, such as myself).  Therefore, the UKF seems a more prudent choice in this case. I might as well note that he chooses the square-root flavor of the UKF (SPKF), to make computation less painful.  My kinda dewd!

In all fairness, I want to point out that my conclusion disagrees with the author's...

Quote

"Considering that in a real mission the initial state of the UAV is quite accurately known, and that the computational resources are restricted, the use of an EKF seems more appropriate in the considered (VTOL) application."

I don't have VTOL in mind, and in the case of a hand-launched UAV, the initial state is a mess and the last thing I want is slow convergence (or worse - divergence), or convergence that's reached as a result of being steady-state (nose buried in the ground, and tail up in the air).

Do you have any experience (even comments based upon a cursory review) with Sebastian Madgwick's work?

PS - van der Merwe has replied, and referred me to two colleagues who are current "keepers of the code."  I'm now making contact with them.

[Haldir]

Lew Payne, on 29 June 2010 - 07:39 PM, said:

(...)
In all fairness, I want to point out that my conclusion disagrees with the author's...
(...)
Do you have any experience (even comments based upon a cursory review) with Sebastian Madgwick's work?
(...)
PS - van der Merwe has replied, and referred me to two colleagues who are current "keepers of the code."  I'm now making contact with them.

I agree with his result that the EKF is better with limited computational capability, but honestly I believe that computational capability is not the issue for a future-proof system,

I've read Mr. Madgwick's description of the algorithm, the math is apparently proof, but unfortunately his comparison lacks descriptions of the used Kalman filter, it's process model etc.
Anyway I think his algorithm is in the vicinity of DCM, both in quality as in computational complexity. I think it's main goal is low computational complexity, which is certainly a nice goal. It won't beat a sr-ukf in my opinion.

On a side note, since Dr. van der Merwe is working for Apple's Advanced Computing Group and the new Iphone 4 has both serious cpu power and all the necessary sensors from his thesis, I wonder if there is a neat little sr-ukf running in the Iphone 4 for the coupling of the different sensors.

[Lew Payne]

Jonathan Brandmeyer, on 29 June 2010 - 01:49 AM, said:

When I started implementing my EKF last fall, I also implemented a UKF state propagation algorithm as well. My finding was that the UKF propagation method consumed 5x additional CPU time, with no improvement whatsoever in initialization or tracking performance.

Jonathan - Standard SPKF's are still computationally expensive.  If m is the number of sigma points required, then for an n-dimension randon state vector x, the "standard" unscented transform needs a set of m = 2n + 1 points to capture the state's statistical distribution properties.  A complete SPKF estimator for the n-dimensional state model is no longer 2n + 1, and easily becomes 4n or larger, mainly because the standard SPKF algorithm requires a state augmentation to include all the propagation and measurement noise terms... hence leading to increased computational burden.

But, that's old school.  Nowadays, the trend is to use a simplex point selection strategy, such as the n + 2 point minimal-skew points, the spherical simplex points, and other high-order extensions.  More recent implementations use a sigma point selection strategy which involves no central-point (Schmidt orthogonalization-based simplex set), and include n + 1 equally weighted sigma point sets which are more stable and accurate (and efficient).  The net effect is that the application of simplex sets have reduced the required sigma points to roughly 50% of the traditional nonaugmented algorithms.  Thus, they have made a significant improvement in numerical efficiency.  I dare to say that the numerical efficiency of a simplex SPKF algorithm can rival or even exceed that of its EKF counterpart... if the usual Riccati equations and numerical integration process are involved in the latter.

While this statement (above) is true, your statement is also true.  Again - it depends on the circumstances (as I often like to say... context is everything).  For typical quaternion-based attitude problems, simple analytical solutions and sparse matrices exist for MEKF' covariance propagation and measurement updates.  Thus, general simplex (again - context) SPKF algorithms still compare unfavorably with MEKF in efficiency.

What we need here is an algorithm which affords a further reduction of m.

I am very happy to see you (and others) doing research in this field, and contributing to what OpenPilot can do!  I think having a diversity of researched opinions on the subject yields benefits to the whole (i.e., the OP community).

[Haldir]

Lew Payne, on 29 June 2010 - 09:37 PM, said:

(...)
But, that's old school.  Nowadays, the trend is to use a simplex point selection strategy, such as the n + 2 point minimal-skew points, the spherical simplex points, and other high-order extensions.  More recent implementations use a sigma point selection strategy which involves no central-point (Schmidt orthogonalization-based simplex set), and include n + 1 equally weighted sigma point sets which are more stable and accurate (and efficient).  The net effect is that the application of simplex sets have reduced the required sigma points to roughly 50% of the traditional nonaugmented algorithms.  Thus, they have made a significant improvement in numerical efficiency.  I dare to say that the numerical efficiency of a simplex SPKF algorithm can rival or even exceed that of its EKF counterpart... if the usual Riccati equations and numerical integration process are involved in the latter.
(...)

I haven't seen the Schmidt orthogonal SPF paper before (apparently the paper is not freely available), just once referenced in the Marginal Geometric Sigma Point filter in http://downloads.hin...2009/507370.pdf (That paper has a nice break-down of the computational complexity of several more modern spf)

That MGSPF has apparently some lower computational complexity, but I'm not sure if their optimizations apply to our rather generic case of a KF.
Also I wonder about it's numerical stability, since for all the other SPF (including spherical simplex) there is a square-root form to reduce certain problems. I don't know if that's not necessary for the MGSPF or just not yet developed.

[Lew Payne]

Haldir, on 30 June 2010 - 06:54 AM, said:

I haven't seen the Schmidt orthogonal SPF paper before (apparently the paper is not freely available), just once referenced in the Marginal Geometric Sigma Point filter in http://downloads.hin...2009/507370.pdf (That paper has a nice break-down of the computational complexity of several more modern spf)

That MGSPF has apparently some lower computational complexity, but I'm not sure if their optimizations apply to our rather generic case of a KF.
Also I wonder about it's numerical stability, since for all the other SPF (including spherical simplex) there is a square-root form to reduce certain problems. I don't know if that's not necessary for the MGSPF or just not yet developed.

Yes... I have the Chunshi Fan paper in my technical blog, where I'm starting to keep notes and comments on some of the papers.  I've added a few more (always at the end) which cover (if memory serves - which isn't too often nowadays) the issue of sample degradation (apparently none - in a proper implementation) and numerical stability.  You may want to look at the last two listed here.  Scaled unscented and novel simplex.  I also dismissed a few other papers recently, as they appeared to be superfluous.  One such paper was on a variable m-point UKF (and either I was not impressed, or not bright enough to realize I should have been impressed).

[Haldir]

Thanks for the paper suggestions. I'll look into them.
At least we won't need all those additive gaussian noise special cases anymore with one of the recent spkf with m+1 or m+2 sigma points. I wouldn't mind getting rid of the square-root versions. The different QR and cholesky methods aren't that readily available outside of matlab and out of my experience of QR it might be one of the reasons nobody did a fixed-point implementation of sr-ukf

Anyway, I guess we should split this Thread and move the general SPKF discussions to a new thread specifically about SPKF, where we can collect all the knowledge and their discussion.

[ajray]

Do you have hardware recommendations for the sensors?  This seems to have some fairly stringent requirements for the sensors.

What did you use while testing? (would you recommend something different?)

[Jonathan Brandmeyer]

ajray, on 30 June 2010 - 01:18 PM, said:

Do you have hardware recommendations for the sensors?  This seems to have some fairly stringent requirements for the sensors.

What did you use while testing? (would you recommend something different?)

Hello from an ARC alum!  I was in the club back in 2005 and 2006, you might find some of my older code in the club's archives.  The "Wing Cammander" camera control system was my product in '05, and I helped MH develop the first version of the current imagery system in '06.

In testing, I have one model that uses a set of premium sensors: ring-laser gyro and a star tracker, for a satellite AHRS simulation. My AHRS (not yet published) nearly equals the performance of Extended QUEST in a head-to-head comparison.

For my GPS-INS, I am assuming the characteristics of a an ADXRS 610 gyro.  The IDG500 will also work, but it has much worse bias stability. See also the Allan Variance plots that I produced with some donated data streams on the DIYDrones forums.