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

Views: 24651

Reply to This

Replies to This Discussion

As i understood,the RMarix outputs measure values of the roll, pitch and yaw which corrected by GPS and acc.
I want to add a "fly by wire" function into DCM.
Following is my PID algorithm.
//From RMarix get corrected values of roll,pitch and yaw.
Meas_roll=(the roll angle from Rmarix);
Meas_pitch=(the pitch angle from Rmarix);
Meas_yaw=(the yaw angle from Rmarix);

//setpoint value is tilt angle of stick of RC transimitter .
Setpoint_roll=(the tilt angle of roll_stick);
Setpoint_pitch=(the tilt angle of pitch_stick);
Setpoint_yaw=(the tilt angle of yaw_stick);
//Error=Measure value-tilt angle of stick.

// Integration of Error_roll

// send above values into mixer() function to control servo.

I don't know whether this PID algorithm can work with DCM.
Somebody could give me a better suggestion about "fly by wire"?
In MatrixNav it seems to be only two kinds of control modes--Automatic and Manual.
How to intergate the stick signal into DCM?


MatrixNav (and AileronAssist) has three control modes: "manual", "stabilized", and "waypoint".

Stabilized mode is very similar to what you are calling "fly-by-wire". It is so stable that you can use it during launch and landing. In particular, AileronAssist can be used to make an unstable plane behave like a "high wing trainer".

In order to make use of the three modes, you need a three position switch controlling the pulse width of the signal going into channel 4 of the board. Manual mode is selected with pulses less than 1.5 milliseconds wide. Stabilized mode is selected with pulses between 1.5 milliseconds wide and 1.75 milliseconds. Waypoint mode is selected with pulses greater than 1.75 milliseconds.

When I fly, I use one of the joysticks to select control mode. I use the trim tab to select manual/stabilized, and the stick to select waypoint mode. Another way is to use a three position switch, but in that case you will probably need to change the firmware to match the positions of your switch. If you use a two position switch, you will need to decide which 2 of the 3 modes you want to use, and modify the firmware accordingly.

The best way to modify the code is to use the latest version of the firmware, now called MatrixPilot, that has provisions for you to customize the mode control. For more information, instructions, and the latest firmware, visit the google project for the UAV DevBoard.

Best regards,
Thanks so much. I will carefully study the latest version source code .
At first I will try to simulate DCM algorithm in MATLAB. I have downloaded the s-function of DCM.
But how to generate gyro/Accelerometer/GPS signals to s-function of DCM in MATLAB?
To use a IMU/GPS simulator or directly get realtime signals from a true IMU/GPS board?

Best regards
Jack Chen
Hi Bill,
I don't know if it's the correct place to ask it, please point me anywhere else in case.
My question is about elevator control, specifically this excerpt from MatrixNav code that computes the pitch rate:

elevAccum.WW = ( __builtin_mulss( rmat[8] , omegagyro[0] ) -
__builtin_mulss( rmat[6] , omegagyro[2] ))1 ;
pitchrate = elevAccum._.W1 ;

From what I understand, pitch rate is the Y coordinate of the cross product between earth Z axis (last row of DCM matrix) and the rotation rates vector.
Is my understanding right ? If yes, could you please explain why pitch rate is computed this way ?
Thank you in advance


Your question is a delightful one. I am going to give you two ways to look at it:

1. One way to look at it is that I use rmat[7] as an indication of pitch. rmat[7] is the cosine of the angle between earth Z axis and the plane's fuselage. When the plane is level, rmat[7] is zero. If you look at the way rmat[7] is integrated, you will see that the time rate of change of rmat[7] is the Y coordinate of the cross product between earth Z axis and the rotation rate vector. This goes all the way back to the primary concept behind the DCM algorithm: the rate of change of a coordinate axis is the cross product of the coordinate axis with the rotation rate vector. Which brings you to another way of looking at it:

2. The time rate of change of the earth Z axis, as seen in the frame of reference of the plane, is the Z axis, as seen in the frame of reference of the plane, times the rotation rate vector. To get the corresponding pitch rate, we need to only look at the fuselage (or Y) component of the rate of change of the Z axis.

Best regards,
Bill Premerlani

All of Paul Bizard's simulations were done entirely in software. The portions that he has published are the IMU algorithms only, not, for example, the dynamics of the plane. For things like gyro and accelerometer, he computes them in his simulation.

Best regards,
Bill, enlightening explanation as ever. The purpose of rmat[7] was clear to me, the rate of change is clear now. What confused me a little was that the fuselage is along Y axis. Summarizing:

proportional term = pitch = cosine of angle between Zearth and Yplane = rmat[7]
derivative term = pitch dot = Y component of cross product of Zearth and omega dot


Zearth = (rmat[6] rmat[7] rmat[8])
omega dot = d omega / dt = (omegagyro[0] omegagyro[1] omegagyro[2])

Thank you
Hello all! I come to you humbly to discuss using magnetometers in the DCM algorithm. I believe that my questions will reflect my lack of understanding here! Currently I am working with the propeller chip and have successfully demonstrated pitch/roll control using the Sparkfun 5dof IMU and the DIYdrones gyro daughter board. So far so good. I also have a GPS available but out of stubbornness I would like to use the HMC5843 3 axis magnetometer as a proof of concept platform.

Ok, so here it goes. Am I going about this correctly? My though process is as follows:

1) To use the three axis magnetometer I need to essentially rotate the magnetic readings from the inertial body coordinate system back to the earth reference frame. (Pitch and roll only.)

2) From there I can compute my heading.

3) Then I can essentially follow the steps from the DCM paper regarding yaw drift cancellation using a GPS signal.

Is this reasonable or is there a much easier way? I feel like there should be....
Yes I know. It's sad when you push the refresh button every ten seconds in anticipation. I need to get a life...

As for the matrix math, My current approach is to take the dot product of the magnetometer vector in the inertial coordinate frame with the first and second rows of the r matrix to rotate it back to the earth reference frame. From there on it's the same principal as the GPS drift cancellation presented in the DCM paper. I've been having a little luck with this method, but as of yet the results are inconsistent. Basically it will be fine for ~2 minutes and then blow up. So. Back to work.

It certainly seems like there should be a simpler way to do it. But for the life of me I cannot figure out what to compare the measured magnetometer data in the inertial frame to in order to develop an error term which can be corrected with a PI controller. <= long sentence.

Any other ideas/experience floating around out there on this one?

You are basically correct.

1. You use the direction-cosine-matrix to transform the magnetic readings from the inertial body coordinate system back to the earth reference frame. If by "pitch and roll only" you mean x and y components only, then I agree. You will need all three of the magnetometer readings. Two dot products will give you the X and Y components in the earth frame. Each row of the matrix represents the projection of one of the earth coordinate axis into the body frame, so you take dot products with the first two rows. You have to be careful figuring out which is X and Y, and the signs, because the paper that I wrote with Paul uses the standard aviation coordinate system, but the UAV DevBoard as the X - Y axis rotated 90 degrees.

2. You determine the yaw error in the matrix by taking the cross product of the measured X-Y magnetic field in the earth frame with the expected X-Y magnetic field in the earth frame.

3. Feed the yaw error into the PI feedback controller that adjusts the yaw error.

The "trick" is going to be being careful with the scaling, which has am impact on the feedback gain. If you turn the overall gain too high, then things will go unstable. For example, in the released firmware for the UAV DevBoard, the roll-pitch error is computed by taking the cross product of measured gravity with "down". So, the result has the magnitude of gravity included, which is taken into account in the feedback gain. In the end, its all about units.

Also, you have to be careful with the sign of the the feedback, if you get that wrong, you'll have trouble. But eventually you should be able to get it to work.

By the way, several folks have already adapted the DCM algorithm to use magnetometers, including Brian Wolfe.

Best regards,
According to DCMDraft2 the GPS alse provides the velocity of planes over ground.
Without gps how to compute the centrifugal acceleration?
And how to amend the RollPitchCorrectionPlane?
The magnetometers seems only to provide the direction of movement.
Bill, thank you again and I am always grateful for your help.

Right now I am using dual pitot tubes in the wings. Why dual? Because two is always better than one. Also, it gives an interesting side slip indication. Good for maintaining into the turn yaw. The magnetometers should help achieve yaw lock before the GPS kicks in (GPS requires movement).


Reply to Discussion


© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service