 |
|
 |
| Parallax Forums > Public Forums > Propeller Chip > Kalman Filter (Working) Small bug | Forum Quick Jump
|
| 
JWood
Software Engineer
      
Date Joined Apr 2007
Total Posts : 56
| Posted 12/22/2007 3:18 PM (GMT -7) |   | |
Hello PropellerHeads,
I'm working on a balancing robot but I ran into a small snag.
I'm using a 5DOF Accelerometer/Gyro combo from spark fun and sending it's values through a Kalman filter. It's working like a champ except for a small problem.
From time to time my q_bias shoots way off beat and it pulls all my other values down with it. I have a graph below that shows my problem. In the graph 180-200 = 2 seconds and the color are explained below. As you can see the angle from the kalman filter (blue line in graph covered up by purple line) is holding tight to the actual angle. Then at 260 seconds (random time) my values go crazy and it's driving me crazy!  Something is throwing my calculations way off base. If it was only a small amount I wouldn’t care but some times it throws my angle 100's of degrees off.
Red = q_bias
Green = angle_erro
Blue = Angle
Yellow = Ignore (PID Control Output)
Purple = Kalman_Update Atan2(xaxis, zaxis)
Grey = xRate
What I know so far. If I don't call the state_update method I don't get the crazy spikes but that kinda makes that entire float math stuff a waste of time. I've checked my values being read from the ADC and they are correct. You can see this from the purple line in the graph above.
I don't know what could be causing this problem. I've been beating me head over it for some time now. I fear that my eyes have become null to the code and I'm over looking something small like a Float Gotcha.
Any way ... I've attached my IMU code. I ported the tilt.c kalman functions from rotomotion's Autopilot software as you’ll no doubtable guess after a few seconds of looking at it.
Thanks for looking and thanks for any help in advance.
Once I get this all up and running I'll post a nice full write up of the entire bot.
Good Day and Happy Holidays.
Jason
Wood
I UPDATED THIS POST WITH THE LATEST AND GREATEST WORKING FILTER!
Post Edited (JWood) : 12/23/2007 5:25:51 PM GMT
File Attachment :
IMU.spin 32KB (application/octet-stream)This file has been downloaded 1477 time(s). | | Back to Top | | |
 |
Perry
Registered Member
Date Joined Jan 2007
Total Posts : 89
| Posted 12/22/2007 4:06 PM (GMT -7) | | How confident are you about getting a 10 Hz update rate.
would the Clock object not be a better way of insuring this
Perry | | Back to Top | | |
|
JWood
Software Engineer
Date Joined Apr 2007
Total Posts : 56
| Posted 12/22/2007 4:32 PM (GMT -7) | | | Thanks for the suggestion Beau.
I tried using the ABS of the counter like this.
dt := fMath.FDiv(fMath.FSub(fMath.FFloat(||cnt), fMath.FFloat(lastTime)), 80000000.0)
lastTime := ||cnt
That gave me an even more unique problem.
I attached an image of me messing with the sensor and then when the glitch happening.
Not exactly what I was looking for. ;)
It repeated like that for some time before stabilizing again. But that does shows me it just may be my problem. Let me look a little farther.
Thanks! Image Attachment :
Abs.bmp 668KB (image/bmp)This image has been viewed 1307 time(s). | | | |
| | Back to Top | | |
|
deSilva
Registered Member
Date Joined Jun 2007
Total Posts : 2972
| Posted 12/22/2007 5:08 PM (GMT -7) | | I had a drawing for that, I do not find at the moment, but will post when it turns up again... I think I burried it in a PowerPoint file...
The trick with handling the overflow of values is computing
newValue - oldValue
The difference is ALWAYS positive when newValue > oldValue, independent of an overflow (the drawing was meant to demonstrate this).
This is sometimes confusing, that a<b should be something different than a-x < a-x. But it is when signed numbers are consideredm, and < works on signed numbers....
You are aware of this, because you utilize it in another place..
When you convert to floating values too early however this effect gets lost, as FLOAT also consideres the correct sign. | | Back to Top | | |
|
Beau Schwabe (Parallax)
IC Layout Engineer
Date Joined Aug 2004
Total Posts : 3967
| Posted 12/23/2007 12:00 AM (GMT -7) | | | | |
|
Fred Hawkins
Registered Member
Date Joined Jun 2007
Total Posts : 996
| Posted 12/23/2007 5:40 AM (GMT -7) | | | Jwood, can you post the corrected code so your first post doesn't mislead? | | Back to Top | | |
|
JWood
Software Engineer
Date Joined Apr 2007
Total Posts : 56
| Posted 12/23/2007 10:00 AM (GMT -7) | |
JohnBF,
A kalman filter is designed for just that. It takes noisy measurements to get a nice smooth angle out of it. It also tracks the bias of your gyro so when it starts walking you don't have to worry about it. I've attached a graph of the working filter. In that graph the Red, Green and Blue bars are the X,Y,Z axis data from the accelerometer in static G Forces. Grey bar is the bias gyro rate after going through the filter. Yellow bar is the filtered tilt angle and purple is the Atan2 angle from the Y and Z axis. This is 20 seconds of measurements and you can see I was sawing the robot from side to side.
I've attached the kalman object in its fully working glory (FINALLY!!!!!! THANKS SO MUCH deSilva). I tried to put in some good comments so you can use what ever setup to match what I have. Depending on the mounting of your accelerometer and gyro you may need to use different axis.
I started out using radiants but switched to degree trying to figure out my bug. It could easily be turned back into radiants if you wanted to.
I'm going to add a few more comments and look at speeding it up before I post it on object exchange. I wanted to add GPS to object making it much more of an IMU. I plan on tracking both pitch and yawn being that my gyro does have two axis to work with. This way I can account for lateral G forces while cornering fast or on an incline.
All,
can I update my first post to remove the code posted?
UPDATE
I see now that I can! Thanks
Jason
Wood
Post Edited (JWood) : 12/23/2007 5:27:13 PM GMT
Image Attachment :
working.bmp 784KB (image/bmp)This image has been viewed 1778 time(s). | | | |
File Attachment :
IMU.spin 32KB (application/octet-stream)This file has been downloaded 1038 time(s). | | Back to Top | | |
|
JohnBF
Registered Member
Date Joined Dec 2006
Total Posts : 101
| Posted 12/23/2007 10:49 AM (GMT -7) | | | Very nicely done. Great comments. Thanks! /John | | Back to Top | | |
|
JWood
Software Engineer
Date Joined Apr 2007
Total Posts : 56
| Posted 12/23/2007 3:47 PM (GMT -7) | | |
Thanks John,
I've read your posts on your ping))) balances bot. That’s good stuff and very encouraging! I'm hopping to finish my bot over this nice long holiday week off!
Got any advice for tuning my PID? My first stab at it resulted in a very unbalanced state.
I just started reading about control theory and P, PI, PID control loops were the first topic. Then I found a nice little PID object in SPIN. That sealed the deal to use it. I understand how, why and what to use it for … Just not really how to tune it.
P is the proportional part of the Process Variable.
ID are both in units of Time but are they directly relational to my update speed? If I put 1.0 and 1.0 for ID would that mean I'm expecting an update every second?
I guess I'm looking more for a good site that explains how to tune a PID. Some quick Google searching got me a site that showed me a little and want's me to buy some software. I want a book to fully understand how a PID works and how to tune it. | | Back to Top | | |
|
Fred Hawkins
Registered Member
Date Joined Jun 2007
Total Posts : 996
| Posted 12/23/2007 3:48 PM (GMT -7) | | | Jason, tell us about your graphing arrangement (they are quite nicely done). | | Back to Top | | |
|
JohnBF
Registered Member
Date Joined Dec 2006
Total Posts : 101
| Posted 12/23/2007 4:26 PM (GMT -7) | | Jason,
I don't know much theory about tuning a PID routine; some of the things I've read have some theory and then they tend to say, but in the end a lot of it is trial and error. I've just gotten right to the trial and error part. My experience with each new version has been something like this...
I generally start with just the P constant set to 1 and D and I turned off (set to 0). As I try bringing bring up P to bigger numbers the bot will sort of balance, but with big swings forward and back - it goes too far forward then recovers and goes too far back, but I usually find a number so that it keeps from falling for 10 or 15 seconds. Then I start bringing up D. This tends to abbreviate the overreactions, but it makes the bot more jerky. It will stay upright for longer, then suddenly slam down. Also, the motors tend to reverse abruptly, so I worry about damaging the motors, and sometimes the wheels skid from the reversals. But usually adding D gets the bot to stay up longer. Then adding I kind of smooths things out.
At this stage I've usually got the bot balancing maybe 80% as good as it gets, but I usually get it a little better with small random changes that have no logic, just random trials.
I've been busy and travelling for work, so I haven't worked on the balancing bot for a couple of months, but your Kalman routine inspired me, so I will get back to it in a few days. When I left off I was experimenting with motors. The servos work nicely and are really simple to use, but the torque and RPMs are very limited. I tried some bigger, faster gearhead DC motors from other projects (mostly from Banebots), but the results have not been good - I'm not sure but I think that these motors have more latency - more play in the gears than the little servos, and this is a real problem for balancing bots.
The really nice balancing bots you see on the web (like nBot) seem to use Pittman motors which list for high prices but I guess some folks know how to get them for more affordable prices. I'm still working on that.
Anyway, good luck and I look forward to seeing your progress.
/John | | Back to Top | | |
|
JWood
Software Engineer
Date Joined Apr 2007
Total Posts : 56
| Posted 1/14/2008 8:51 AM (GMT -7) | | simon,
insanity lead me to hook up VDD to channel 6. It isn't needed and I removed it from my lastest that I'm about to release.
Your problem may be a bug in the code.
Try this:
changed:
yAxis0 := tiltReadings[supply] / 2 - 55
to:
yAxis0 := MCP.in(supply) / 2 - 55
I've jared my 5DOF maybe alittle to hard to many times so my Axis 0'ing is a little off. Thats my -55 I've commented that in the future release.
I've had other people claim success using this code.
Try and use the MCP3208 and read the values from your 5DOF directly. Just to make sure all is working.
I'm going to try and make this Object and Abstracted as possable. So you don't need to make any code change to the Object when you get it. Just throw a few variables at it and it run.
Let me know if this helps any. | | Back to Top | | |
| 173 posts in this thread.
Viewing Page : 1 2 3 4 5 6 7 |
| Forum Information | Currently it is Thursday, July 29, 2010 5:16 PM (GMT -7)
There are a total of 462,439 posts in 62,066 threads.
In the last 3 days there were 90 new threads and 802 reply posts. View Active Threads
| | Who's Online | This forum has 20143 registered members. Please welcome our newest member, ME01.
52 Guest(s), 9 Registered Member(s) are currently online. Details
John Abshier, Rayman, kf4ixm, BradC, Sapieha, Gene Bonin, laser-vector, localroger, Nick McClick |
Forum powered by dotNetBB v2.42EC SP2.02
dotNetBB © 2000-2010 |
|
|