Kalman filter question in matlab

[tranzsport]

Hey guys,
I've got a quick kalman filter question, hopefully its nothing too much.
I've been using the rotomotion kalman filter by Tom Hudson, the matlab version, to filter my own imu data.

here's the link to the original code i'm adjusting:
http://gear.chuckpaiwong.com/files/inertial/1dof/tilt.m

The code filters only one axes of rotation (1dof) , and is supposed to fuse roll(or pitch) rate from the gyro with the roll(or pitch) angle as calculated from the accelerometer g's.
I've adjusted the code so that it takes all the input data from a text file that is output from my microcontroller.
the text file has 10 columns described as follows:

time-stamp(s) rollrate(deg/sec) pitchrate yawrate accel-x(in g's) accel-y accel-z theta_roll(degrees) theta_pitch theta_yaw

as it stands, the microcontroller is successfully calculating the last three columns as follows:
the roll and pitch are being calculated directly from the accelerometer g's with high accuracy, while the yaw is directly from the third gyro. but u can disregard the yaw as i'm not filtering this since i'm only using only 1 tri-axis accelerometer.

I'm feeding the roll rate (or pitch rate) and theta_roll (or theta_pitch) and also time columns as being the three inputs to the matlab code.

The code successfully graphs the angle from the accelerometer measurement, and it also successfuly graphs the same angle as calculated in matlab using the gyro rate (u can vividly see the drift from the gyro).

Now, the problem is this:

no matter what I input for the gyro rate, the kalman estimate output that I'm getting from the code is always following that of the accelerometer reading, and the estimate completely disregards the gyro portion of the process, no matter what it is.
I've been trying to fix the code and adjust the kalman equations such that I have some sort of "fusion" in the estimate but without success.

sorry if this is confusing in anyway, but i'm desperately trying to get this to work in time for my project deadline.

Could anyone please help me with this?

Thanks a lot.

PS: the data represent rolling +/- 90 degrees, then pitching +/- 90 degrees, then yawing +/- 180 degrees. there is a small pause between each movement.

[m1tch37]

I don't know the origin of that script, but it is clearly based off the Rotomotion tilt.c. I would give the original a shot:

http://www.rotomotion.com/downloads/tilt.c

Best of luck.

[dnile]

Have you tried adjusting the covariances for the different inputs? You can even adjust them by ridiculous amounts just to see if the Kalman filter is doing anything at all.

If that doesn't work, I'd suggest writing the filter yourself, because that way you'll find it much easier to examine your code and find out what's going wrong. A linear Kalman filter isn't that hard to implement so long as you write it carefully. I'd suggest checking out the wikipedia page on Kalman filters to get started.

You might also want to try writing it in C and compiling it as an S-function, because that way you can easily port it over to whatever microcontroller/microprocessor you're using.

Cheers,
Daniel

[Unterhausen]

I think I understand what you say your problem is, does the example data really show the problem? When you say "actual", do you mean the Kalman estimate?

[wandroid]

So I gather that in your data, column 6 (starting with left-most as 1), the Y axis is the accelerometer that corresponds to the roll in the gyro column 2. I have a copy of the original tilt.c code from a Nuts-N-Volts website that is a hacked version for a single axis balancing robot project. I was thinking of trying your data from column 2 and 6 of your file to see if I could get the single axis version to produce something reasonable. In other words, I'm thinking that using the column 2 and 6 data could be viewed as if it were sensor data from the single-axis bal-bot. This might make a much simpler debug case on a version of the code that's allegedly working in a simpler application.

BTW, can I assume that your samples are a consistent 29ms each? Looks like it from the few I checked.

[wandroid]

I plotted the roll axis rate gyro (blue) and y-axis accelerometer (black) in an XL plot. I noticed that your data looks very similar to data I've collected using a SparKFun two axis IMU. One problem I had is that the gyro data appears to be very noisy. I'm not sure if that's ok or not. Is this just the drift that's always mentioned for these gyros? If so, it happens very quickly. I ended up filtering the gyro data a bit with a simple IIR filter, but I'm not sure that's a valid thing to do. Anyway, will try to get some output from the code I mentioned.

[Unterhausen]

I think you probably have to filter the gyro. They are rate gyros, so it gets integrated somehow before incorporation into the position. Integration has the effect of filtering the data to some degree.

When you run his program, the gyro output diverges from zero, which represents the bias.

He probably doesn't need us any more, but I am thinking he doesn't really have a problem.

[wandroid]

For what it's worth, I do seem to be getting angle and gyro bias data out of that tilt.c code. I put together a simple main to drive the function calls and feed the RollRate and y-axis data. I had to change the scaling parameters since the original code worked directly with raw ADC values and not acceleration in g. The yellow plot is the angle, green is the gyro bias. The RollRate and Y-axis data are there, but get swamped out by the large relative angle output plot. The blue and red plots are essentially the same RollRate and Y-axis data I put in the previous post. I guess I called the red one black in the previous post. Sorry, I'm color blind 8)

So maybe I'm missing the point, but it seems that if you just roll the sensors smoothly while basically still in every other regard, the angle is going to just pretty much track the acceleration data, although you can see what appears to be the gyro drift being tuned out. I thought the key point of the filter though is that if the whole system is in motion, such as being bounced around in an aircraft, the resulting accelerations that are NOT due to the rotation angle are also tuned out by the filter, leaving you essentially with an artificial horizon. The real magic is that it's combing the gyro data with the accelerometer data in such a way as to tune out everything except what's relevant to generating an angle estimate. In other words, the filter is able to resolve the problem of not being able to distinguish the aircraft accelerations from the acceleration of gravity due to tilt, by combining the rate gyro data in the estimate. Do I have that right?

So to really test it, it seems you would need to be bouncing this thing around a bit while applying rotation, and see that the angle estimate is not affected by the other motion.

[Unterhausen]

In other words, the filter is able to resolve the problem of not being able to distinguish the aircraft accelerations from the acceleration of gravity due to tilt, by combining the rate gyro data in the estimate. Do I have that right?
I've quoted your main contribution to the discussion, I believe you are correct. Resolving the gravity vector is a very important aspect of the filter.

[joroge]

It seems that the Rotomotion code does two different things.
1. An AHRS part using the gravity vector and the magnetic vector to find pitch,roll and yaw and then use this calculations as
measurements to correct the gyro propagation.
2. A navigation part using GPS measurements to correct the position and velocity propagation (accelerometers).
So, there is one filter for AHRS and one for NAV. With the first one, you will find the Attitude and heading and with the other one you 'll find position and velocity.
You may want try to adjust your Po,Xo,R and Q. Do it for the attitude first and then for the NAV, this b/e NAV uses attitude estimation. R is related to the sensors (variances) and Q is called process noise, which is an artificial way add noise to the filter. The best way to do the fine tunning of the filters, is to plot the sqrt(diag(P)) vs X-X_hat (estimation error); this plot will tell you if the filter is actually keeping your states (position,velocity, attitude, gyro bias, acc bias) bounded under the specified limits.

[poynting]

The real magic is that it's combing the gyro data with the accelerometer data in such a way as to tune out everything except what's relevant to generating an angle estimate. In other words, the filter is able to resolve the problem of not being able to distinguish the aircraft accelerations from the acceleration of gravity due to tilt, by combining the rate gyro data in the estimate. Do I have that right?

You're going down the right path, and while you're not quite incorrect, your explanation is incomplete. If the gyros were perfect, this might be true (and of course you wouldn't really need the accels), but in fact what we're doing by using the Kalman filter (or a complimentary filter if you want something a little more simple) is to accept that the gyros are good at one thing and have relatively high drift, while the accelerometers are good at another and average over time to measure gravity. The two are related in some way, and the combination of the two tells you more than you would know without one or the other. If your gyros are poor, you'll have to trust your accelerometers more, but they can't do anything but measure all of the accelerations they're exposed to. How complicated the filter is and what other information you provide it will determine if the filter can distinguish between accelerations due to aircraft movement/orientation and acceleration purely due to gravity.

As an example, take the case where the aircraft accelerates along the forward axis, with no rotation, such as when you apply throttle in a neutrally stable aircraft. If you don't include in the filter some model of the acceleration due to a change in throttle, the filter will "assume" that this acceleration is due to gravity, and when it sees acceleration in a direction other than down, it will have some information that is telling it that the attitude has changed. (It will also have the gyro which has told it that the attitude has not changed.) Now, if the acceleration was only temporary, we can trust that the response from the accelerometers will only be short-lived, and the filter, having not seen a change in pitch rate to integrate, will only develop a small error in the pitch estimate. For longer lived accelerations however this may not be true, and the result will be that the filter develops an error. In some cases this type of error can result in positive feedback systems that develop instabilities or diverge completely.

That said, there's really no magic in the filter... it's very elegant but all it's doing is taking information you give it, combining it in an optimal way (optimal in the sense that it's minimizing the sum of the squared error between it's estimate and it's measurements), and then giving you an estimate of some parameter along with a measure of trust in that parameter (via the covariance matrix). In some cases it will (magically or not) give you a very bad estimate, but if it has been tuned correctly it will at least let you know that it is doing a poor job (assuming it doesn't diverge completely in the process).

So, the point being that with just gyros and accels, the filter can't know anything other that what you've told it and what it's measuring. If you want it to be able to distinguish random motion you have to give it more measurements (such as airspeed, GPS, magnetometers, etc) and/or you have to refine the transition and input models, i.e. model the system such that it is an airplane (rather than a car or helicopter or a bicycle) in forward flight under "normal" conditions.

[hbaocr]

Hey guys,
I've got a quick kalman filter question, hopefully its nothing too much.
I've been using the rotomotion kalman filter by Tom Hudson, the matlab version, to filter my own imu data.

here's the link to the original code i'm adjusting:
http://gear.chuckpaiwong.com/files/inertial/1dof/tilt.m

.
I'm newcomer in UAV.I have done that code to estimate my flight test data.It maybe is very good.But I doubt that .Only with 6 DOF IMU (3gyro,and 3 accelerator) ,the angle roll and pitch estimate from that code is only inclination angle of X axis and Y axis,it isn't the attitude angle_pitch and roll ,is it???and which way we can estimate the true attitude of my flight .

[rbeall]

I didn't solidly look the code but I know I had something similar when I tackled this project a year a go. When ever you integrate you must carry forward the new number with a running int. Same with the running kalman.

ex:
x=x+dtx or what ever doesn't really work in matlab

you have to do something like this

for i<lengthdata
x(i)=x(i-1)+dtx(i-1);
end

hope that helps may not even be what you are asking but either way that's my short glimpse into the problem...

Secondly don't be fooled by people calling their filters Kalamns used for flight when they only look at single axis integration. In the 6 degrees of freedom they are coupled and you have to account likewise in most applications

[Paul_BB]

Hi,

I've also programmed on Matlab/simulink the EKF from Rotomotion for means of comparison with the DCM algorithm we developped with Bill Premerlani on DIY Drones.

It works pretty good. If you want the S-Function I can send it to you.

Paul

[hbaocr]

Hi,

I've also programmed on Matlab/simulink the EKF from Rotomotion for means of comparison with the DCM algorithm we developped with Bill Premerlani on DIY Drones.

It works pretty good. If you want the S-Function I can send it to you.

Paul
please share it to me!thanks you so much;
my email duonghuynhbaocr@gmail.com!
I'm a researching in UAV! I'm really a newcommer ! have just researched it for 2 months!

[Paul_BB]

please share it to me!thanks you so much;
my email duonghuynhbaocr@gmail.com!
I'm a researching in UAV! I'm really a newcommer ! have just researched it for 2 months!

You're welcome, I've just send it to you. :)

Some improvement can be done by taking into account a mean AngleofAttack.

Paul

[gwfalcon]

Dear Paul:
Could you share your S-function with me also? I am also involved in such a project.
Many thanks.

My email: guoweifalcon@gmail.com
Alex