

Thread OP

Discussion
Kalman filter question in matlab
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: timestamp(s) rollrate(deg/sec) pitchrate yawrate accelx(in g's) accely accelz 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 triaxis 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. 






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. 





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 Sfunction, because that way you can easily port it over to whatever microcontroller/microprocessor you're using. Cheers, Daniel 





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?






So I gather that in your data, column 6 (starting with leftmost 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 NutsNVolts 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 singleaxis balbot. 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. 





I plotted the roll axis rate gyro (blue) and yaxis 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.


Last edited by wandroid; May 06, 2008 at 12:10 PM.




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. 





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 yaxis 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 Yaxis 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 Yaxis 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. 

Last edited by wandroid; May 06, 2008 at 12:21 PM.




Quote:







AHRS description
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 XX_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. 

Last edited by joroge; Jun 08, 2008 at 06:05 PM.




Quote:
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 shortlived, 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. 






Quote:



Last edited by hbaocr; Aug 23, 2009 at 10:51 AM.





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(i1)+dtx(i1); 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 





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 SFunction I can send it to you. Paul 

Last edited by Paul_BB; Aug 23, 2009 at 07:50 AM.




Quote:
my email [email protected]! I'm a researching in UAV! I'm really a newcommer ! have just researched it for 2 months! 



Thread Tools  
Similar Threads  
Category  Thread  Thread Starter  Forum  Replies  Last Post 
Can I ask a radio question in here?  Sloper59  Slope  48  Sep 05, 2004 04:57 PM  
PJS300 specific question... (In a TM and later a Molt Hellcat)  Trikster  Power Systems  9  Jul 15, 2003 06:45 PM  
Will a wattage IC5A ESC handle 2X EDF50's??? (same question in Jets forum)  mike93  Foamies (Kits)  0  May 17, 2002 09:14 PM  
Same question in multiple forums?  ClausT  Site Chat  3  Apr 22, 2002 05:44 AM 