If you downloaded MatrixNav from this page before 4/29/2009, you should be aware that there is a newer version of the firmware, MatrixNavRv2, that reduces the GPS latency, and will perform much better than the first version.I have been working with Paul Bizard on something we call the "Premerlani-Bizard robust direction cosine matrix estimator". It is based on the work of Mahony et al. The idea is to continuously update the 3X3 matrix that defines the relative orientation of the plane and ground reference frames, using GPS and 3 gyros and accelerometers. The basic idea is to use gyro information as the primary link between the two reference frames, and to use GPS and accelerometer information to compensate for gyro drift. We are working on the theory together. Paul is performing simulations. I am testing ideas in my UAV DevBoard. We have made a great deal of progress. There are demos available, and control and navigation firmware is available. The steps of the algorithm are:1. Use the gyro information to integrate the nonlinear differential equations for the time rate of change of the direction cosines.2. Renormalize the matrix to guarantee the orthogonality conditions for a direction cosine matrix.3. Use speed and gyro information to adjust accelerometer information for centrifugal effects.4. Use accelerometer information to cancel roll-pitch drift.5. Use GPS information to cancel yaw drift.By the way, the algorithm should work in any GPS, gyro, accelerometer nav system on a plane. Without magnetometer information, it will not work on a helicopter.This discussion will provide progress reports from time to time. At this point we have completed all steps. Firmware and documentation for various demos and flight firmware are available on the UAV DevBoard main page.Firmware and documentation of a roll-pitch-yaw demo program are available. There is also a first draft of an explanation of the algorithm.If you have a UAV DevBoard, I highly recommend that you try the demo program, it is very easy to use, and runs without a GPS. During its development, I found that the gyro drift was much less than I thought it would be. After I added the drift compensation, the resulting roll-pitch peformance is nothing less than astounding.Flight testing of "MatrixNav" is also complete. Firmware and documentation are available on the UAV DevBoard main page for stabilization and return-to-launch functions for inherently stable aircraft that are controlled by elevator and rudder. MatrixNav is implemented with a direction cosine matrix, and supercedes GentleNav. Anyone who has GentleNav should replace it with MatrixNav. Pitch stabilization is excellent under all conditions. Return to launch performance is excellent under calm conditions, and good under windy conditions. If you have the UAV DevBoard and an inherently stable plane, you will definitely want to try out MatrixNav.Finally, AileronAssist, for the stabilization and RTL aircraft that have ailerons, is available.What Paul and I are going to tackle next is altitude control.Bill Premerlani
You need to be a member of diydrones to add comments!
I am interested in getting the DCM Lite to run on the parallax propeller. I have a 5 DoF board, is that sufficient to run the DCM lite code?? As I understand the the DCM lite code only estimates pitch and roll, however dont you still need the yaw gyro to make accurate estimations of pitch and roll.
I have "Programmed" the DCM estimation algorithm in Matlab and seem to understand how it is working. The next step I am taking is translating it to the Propeller chip. Using floating point operations and the full renormalization scheme, I am getting a clock speed of ~25Hz!!! AHHHH!!! So I have a few questions...
First, is it at all reasonable to assume that the DCM matrix is symmetric? (Cuts out half the math operations.) From what I can see with the Matlab file this is not entirely accurate, but has anyone else played with this?
Second option, is anyone here versed enough with the propeller chip to propose a faster floating point matrix multiplication scheme?
I finally read your paper "DCM first draft" and I have a question.
In Eqn 25, in order to estimate the Acentrifugal, you make the assumption that, in the plane reference frame, the velocity vector is [GPS Speed, 0, 0]. Well l I have a problem undestanding how it can handle a continuous turn in windy conditions. Are you relying on the fact that over a long period of time it averages out ? For exemple let's say we have a plane TAS = 40 km/h and a 30 km/h wind. During the upwind part of the turn the GPS speed is low then Acentrigufal is computed as very low therefore the DevBoard sees the Gravity as being almost parallel to the Zframe vector and that regardless of the actual bank, it will then correct the gyros accordingly and the bank will increase. Obviously I know that the opposite will occure during the downwind leg and it should bring the bank back the correct level.
So I suppose that it all comes down to the characteristics of the PI correcter ,the period has to be long enough right ?
I'm a fan of your work and just want to fooly ;-) understand your theory and how you can work without an airspeed sensor or estimation of the TAS which for a full sized pilot like me sounds amazing.
Bill, do you know if anyone has made significant progress in porting your method over to the Arduino? I'm getting ready to do so myself but certainly wouldnt mind if someone else had already done some/all of the heavy lifting.
I have a bit of a different take and project I'm working on - not flying, but on the ground/water.
the idea is to record accel/gyro/mag sensor readings then run these through a program and reconstruct the path that the sensor took.
I'm completely new to this 6DOF/DCM stuff. So, I expect to ask some silly questions. Sorry in advance.
Does anyone have a matlab/simulink version of the DCM implemented here ? Or maybe a PC version of the DCM code ?
FYI - it's for a good cause. This is being used for a study of autistic children, and animal behavioral research (very non-profit - as in out of my pocket / donated time kind of non-profit). Think of putting a simple sensor on, that only records the sensors, for hours. Then we analyze the movements later. We don't need to do this real time on the sensor board, as there is nothing to control.
Whatever analysis I get done I'll be glad to post back. I would love to have a matlab/simulink simulation setup, so that I could play with different A/D configurations / different filter configurations / different update rates and on and on.
hello all,
I am new to this forum and have a question. Does any one know a method to compensate the gyro drift using particle filters in the manner kalman does it...using both the gyro readings and calculated angles from acclerometers. I need to compensate the drift using particle filter only...! Any help would be very much appreciated. Also if you could refer to some reading material on the topic then it would be great of you... I have tried reading different resources but maths involved is way too high for me... :(
I just had quick read through ur paper. How does it compensate for sidewind? Gps give out course over ground, but if there is wind the tail needs to be paralell to the total speed component.
And how is it possible to extract winddirection when you know airspeed, groundspeed and course over ground, ?
@William: You are most generous. I have now downloaded the draft, and will be digesting it this evening (and probably several evening hence!). I'm sure I'll have questions ;-)
That is some impressive work done by the Premerlani-Bizard team and of course the rest of the people taking their work forward to new places.
Can anyone share a code in a generic form? Floating point C or Matlab (Sean, Jose…)
I think it will help many people struggling with Mahony et al and the fabulous fixed point coding Bill has done.
I'm working on a floating point PC port of DCM right now. Identifying the extra/missing constants related to switching between the 1.15 and 2.14 formats required figuring out some of the underlying math, which was enjoyable, but took longer than I'd like to admit.
Here's one example. rmat contains 3 orthonormal vectors; one per spatial axis. These rotate to represent the plane's orientation vs an external reference frame (usually Earth). See the great explanations earlier in this thread.
It is critical that these vectors remain orthogonal (perpendicular) to each other, and also, that they remain normal (length=1), where length is sqrt(x^2+y^2+z^2). Without infinite precision math, eventually they will become non-normal, and this will quickly cause a numerical saturation or overflow in the fixed point types. It's a problem in floating point as well; other parts of the implementation use simplifications which depend on length=1.
To renormalize we just need to divide each vector by its scalar length. x^2+y^2+z^2 is easy to compute but how about the division and sqrt()? Bill uses a simple but surprisingly accurate approximation.
Using Newton's Method we can approximate 1/sqrt(S) iteratively with
Xn+1 = (Xn / 2) * (3 - S*(Xn)^2))
Where Xn is the current estimate and Xn+1 is the next estimate, and S is the function input.
What makes this great for a nearly-normal vector is that the initial estimate (X0) is 1.
So, X1 = 1 / 2 * (3 - S*(1)^2)
Simplifying,
X1 = (3/2) - (S / 2)
That's simple enough. 1 subtraction and 1 shift for a fixed point type.
How well does 1 iteration work?
octave:21> format long
octave:22> 1/sqrt(0.9)
ans = 1.05409255338946
octave:23> (3/2)-(0.9/2)
ans = 1.05000000000000
octave:24> 1/sqrt(1.05)
ans = 0.975900072948533
octave:25> (3/2)-(1.05/2)
ans = 0.975000000000000
octave:26> (3/2)-(1.001/2)
ans = 0.999500000000000
octave:27> 1/sqrt(1.001)
ans = 0.999500374687773
This is running after every sensor update and it does a good job of renormalization, very quickly with very little code.
Replies
FYI here is a fixed point object for the propeller
http://obex.parallax.com/objects/501/
I have "Programmed" the DCM estimation algorithm in Matlab and seem to understand how it is working. The next step I am taking is translating it to the Propeller chip. Using floating point operations and the full renormalization scheme, I am getting a clock speed of ~25Hz!!! AHHHH!!! So I have a few questions...
First, is it at all reasonable to assume that the DCM matrix is symmetric? (Cuts out half the math operations.) From what I can see with the Matlab file this is not entirely accurate, but has anyone else played with this?
Second option, is anyone here versed enough with the propeller chip to propose a faster floating point matrix multiplication scheme?
I finally read your paper "DCM first draft" and I have a question.
In Eqn 25, in order to estimate the Acentrifugal, you make the assumption that, in the plane reference frame, the velocity vector is [GPS Speed, 0, 0]. Well l I have a problem undestanding how it can handle a continuous turn in windy conditions. Are you relying on the fact that over a long period of time it averages out ? For exemple let's say we have a plane TAS = 40 km/h and a 30 km/h wind. During the upwind part of the turn the GPS speed is low then Acentrigufal is computed as very low therefore the DevBoard sees the Gravity as being almost parallel to the Zframe vector and that regardless of the actual bank, it will then correct the gyros accordingly and the bank will increase. Obviously I know that the opposite will occure during the downwind leg and it should bring the bank back the correct level.
So I suppose that it all comes down to the characteristics of the PI correcter ,the period has to be long enough right ?
I'm a fan of your work and just want to fooly ;-) understand your theory and how you can work without an airspeed sensor or estimation of the TAS which for a full sized pilot like me sounds amazing.
Best regards,
Dave
Thanks
I have a bit of a different take and project I'm working on - not flying, but on the ground/water.
the idea is to record accel/gyro/mag sensor readings then run these through a program and reconstruct the path that the sensor took.
I'm completely new to this 6DOF/DCM stuff. So, I expect to ask some silly questions. Sorry in advance.
Does anyone have a matlab/simulink version of the DCM implemented here ? Or maybe a PC version of the DCM code ?
FYI - it's for a good cause. This is being used for a study of autistic children, and animal behavioral research (very non-profit - as in out of my pocket / donated time kind of non-profit). Think of putting a simple sensor on, that only records the sensors, for hours. Then we analyze the movements later. We don't need to do this real time on the sensor board, as there is nothing to control.
Whatever analysis I get done I'll be glad to post back. I would love to have a matlab/simulink simulation setup, so that I could play with different A/D configurations / different filter configurations / different update rates and on and on.
Thanks
Scott K.
I am new to this forum and have a question. Does any one know a method to compensate the gyro drift using particle filters in the manner kalman does it...using both the gyro readings and calculated angles from acclerometers. I need to compensate the drift using particle filter only...! Any help would be very much appreciated. Also if you could refer to some reading material on the topic then it would be great of you... I have tried reading different resources but maths involved is way too high for me... :(
Awaiting ur swift reply...!
And how is it possible to extract winddirection when you know airspeed, groundspeed and course over ground, ?
Do u use the acceloremeter to take out the speed?
Kim
Thank you.
Can anyone share a code in a generic form? Floating point C or Matlab (Sean, Jose…)
I think it will help many people struggling with Mahony et al and the fabulous fixed point coding Bill has done.
ManorA
Here's one example. rmat contains 3 orthonormal vectors; one per spatial axis. These rotate to represent the plane's orientation vs an external reference frame (usually Earth). See the great explanations earlier in this thread.
It is critical that these vectors remain orthogonal (perpendicular) to each other, and also, that they remain normal (length=1), where length is sqrt(x^2+y^2+z^2). Without infinite precision math, eventually they will become non-normal, and this will quickly cause a numerical saturation or overflow in the fixed point types. It's a problem in floating point as well; other parts of the implementation use simplifications which depend on length=1.
To renormalize we just need to divide each vector by its scalar length. x^2+y^2+z^2 is easy to compute but how about the division and sqrt()? Bill uses a simple but surprisingly accurate approximation.
Using Newton's Method we can approximate 1/sqrt(S) iteratively with
Xn+1 = (Xn / 2) * (3 - S*(Xn)^2))
Where Xn is the current estimate and Xn+1 is the next estimate, and S is the function input.
What makes this great for a nearly-normal vector is that the initial estimate (X0) is 1.
So, X1 = 1 / 2 * (3 - S*(1)^2)
Simplifying,
X1 = (3/2) - (S / 2)
That's simple enough. 1 subtraction and 1 shift for a fixed point type.
How well does 1 iteration work?
octave:21> format long
octave:22> 1/sqrt(0.9)
ans = 1.05409255338946
octave:23> (3/2)-(0.9/2)
ans = 1.05000000000000
octave:24> 1/sqrt(1.05)
ans = 0.975900072948533
octave:25> (3/2)-(1.05/2)
ans = 0.975000000000000
octave:26> (3/2)-(1.001/2)
ans = 0.999500000000000
octave:27> 1/sqrt(1.001)
ans = 0.999500374687773
This is running after every sensor update and it does a good job of renormalization, very quickly with very little code.
-
3
-
4
-
5
-
6
-
7
of 7 Next