Kalman Filter (Working) Small bug

JWood · 2007-12-22 22:18

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

Perry · 2007-12-22 23:06

How confident are you about getting a 10 Hz update rate.
would the Clock object not be a better way of insuring this

Perry

Beau Schwabe · 2007-12-22 23:08

My suspicion is that when the Counter rolls over from $FFFF_FFFF to $0000_0000, it may be causing the "glitch" that you are experiencing. Referencing the counter as an absolute value might solve your problem.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe

IC Layout Engineer
Parallax, Inc.

JWood · 2007-12-22 23:32

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. [noparse];)[/noparse]

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!

deSilva · 2007-12-22 23:33

Yes, Beau, its line 318:

    dt := fMath.FDiv(fMath.FSub(fMath.FFloat(cnt), fMath.FFloat(lastTime)), 40000000.0)

should read

    dt := fMath.FDiv(fMath.FFloat(cnt-lastTime), 40000000.0)

This here is handled correctly:

pidCalcTime := cnt - (clkfreq/10)
  kalmanCalcTime := cnt - (clkfreq/100_000)

..... on line 334, although I have my doubts that all those FloatToStrings can be computetd in 100 ms....

I cannot find kalmanCalcTime, though....

JWood · 2007-12-22 23:49

deSilva!
Something so small!
·
dt·:=·fMath.FDiv(fMath.FFloat(cnt-lastTime),·40000000.0)
Tell me ... what difference is it to turn two int's into floats and subtract them apposed to subtracting two int's and turning them into floats? That is, other than the obvious waste of instructions.
It's been running from when you posted this till now and hasn't glitches once.
Thanks a bunch!

deSilva · 2007-12-23 00:08

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.

JWood · 2007-12-23 00:18

yes deSilva I was aware of this after reading the FloatMath documentation. Now that you mention it I do remember reading about it always being positive. I'll pull out the documents and go over them again.

I don't know why I missed that ... Like I said ... I read the code too much and I was no longer able to be bias towards it.

Thanks you VERY much. It's still working perfect.

Now on to the PID control loops.

Beau Schwabe · 2007-12-23 07:00

JWood,

Glad that you got it working.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe

IC Layout Engineer
Parallax, Inc.

Fred Hawkins · 2007-12-23 12:40

Jwood, can you post the corrected code so your first post doesn't mislead?

JohnBF · 2007-12-23 12:42

I built a propeller robot that balances·using·a Ping))) sensor to determine·tilt. See my post at:

http://forums.parallax.com/showthread.php?p=645097

I added an accelerometer and rate gyro but so far I have been unsuccessful in finding a way to to integrate them and overcome their particular combination of noise/drift. If·your spin·Kalman filter·winds up as a·an object in the Object Exchange·I'd be thrilled.

Jason, for my project, a very simple version of the PID routine works just fine, so I'm sure that won't·hold you up very long. Please post more when the project is done!

/John

JWood · 2007-12-23 17:00

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

JohnBF · 2007-12-23 17:49

Very nicely done. Great comments. Thanks! /John

JWood · 2007-12-23 22:47

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.

Fred Hawkins · 2007-12-23 22:48

Jason, tell us about your graphing arrangement (they are quite nicely done).

JWood · 2007-12-23 23:15

I'm a software programming by profession and I don't like paying for software. So many times I write my own.
I'm writing a software package that will allowing any type of graphing to exist in the same space. They can also react off of events from one another. So for instance you could be graphing the input from the Parallax USB Oscilloscope and fire events based on rising/falling peaks and apply that to a liner filter on inputs from a serial stream.
I'm only writing some proof of concepts but I've got a decently working tool. As of right now it can do serial graphing and the Parallax USB Oscilloscope graphing. It still has a LONG way to go.
If you would like to download and try it you can from here jtw.programmer.googlepages.com/mathGraphing.zip. Top Toolbar button is a serial graph and bottom is an oscope graph. It will insert a graph and you can move it by clicking on it and dragging it. You also change its settings and connect it to the serial port by clicking on the graph. That brings up settings in the pain to the right. Holding down the middle mouse button allows you to pan and scrolling the wheel zooms in and out.
·Let me know what you think and if this is worth making stable.

JohnBF · 2007-12-23 23:26

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

JWood · 2007-12-24 00:57

John,
Thats a great start. It's way more than I had before and sound right up my path. Trial and error. [noparse];)[/noparse]

simonl · 2008-01-14 10:04

Hi Jason,

This is excellent work - thank you for sharing

In your code you have:

gearBox       : "motion"
PID           : "PID"

I'm assuming the PID object is the one by Andy Lindsay?

I wonder if you'd mind sharing your 'motion.spin' too, so I can test ?

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,

Simon

www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
BTW: I type as I'm thinking, so please don't take any offense at my writing style

JWood · 2008-01-14 15:22

Thanks Simon!

I found a few bugs in the Kalman code and a few people have pointed some out as well. So I'll be releasing a 1.0 in Object exchange here this week with correct stack space.

Yes I'm using the PID object from Andy Lindsay and it's awesome.

As for "motion" it's home brewed. I glued a disk with Black/White strips every 15 degrees to both wheels·and I'm bouncing IR light off to tell RPM of each wheel. I'm passing that info through a PID per wheel and adjusting PWM accordingly. This gives a much better reaction times·to my balance PID loops.

I'll post it tonight when I get home. I'm sure there is 100 better way to encode wheel movement but what I've done is cheap and easy. Only drawback is it·requires a COG to watch the pulses from the IR transistors.

simonl · 2008-01-14 15:33

OK Jason, I'll await the updated version

In the meantime; I've hooked-up the IMU & MCP3208 as shown in the code, but I'm not getting the sorts of readings I'd expected (e.g. after a short while I get an 'angle' reading of 1110704109 - not quite in the 0-359deg I was expecting! And the bias and rate readings are equally nonsense :-()

I'm sure it must be something I've missed or not done right! Do you have any ideas what I've (not) done?

Also; what's the reasoning behind connecting MCP channel 6 to VDD/3.3V - is it as a reference point?

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,

Simon

www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
BTW: I type as I'm thinking, so please don't take any offense at my writing style

JWood · 2008-01-14 15:51

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[noparse][[/noparse]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.

simonl · 2008-01-14 15:54

OK, will do. I'll try it again tonight. (I should've thought of getting the raw MCP readings before I posted - oops!)

Many thanks for the reply.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,

Simon

www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
BTW: I type as I'm thinking, so please don't take any offense at my writing style

JWood · 2008-01-14 15:58

Very good Simon.

I'll be on tonight to give you more of a hand if you need it.

Let use know how it goes.

Good Day
Jason
·· Wood

simonl · 2008-01-17 14:11

Hi Jason,

Not having much luck I'm afraid :-(

I've created a test harness to see the raw data from the IMU to MCP3208 (see attached spin archive). I'm getting readings in the range of about 1500 to 2500 (see attached graph) - except for yRateRaw, which appears to be faulty (I think it's my soldering, as the readings change significantly when I gently press on the edge i=of the IMU!). Is this range what you'd expect?

As you'll see in the attached, I'm taking the readings at 50Hz, with each channel being a 10-point average read sequentially. I'm hoping that'll be good enough to run into the Kalman Filter for use in an autopilot?

I'd appreciate any help you can provide.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,

Simon

www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
BTW: I type as I'm thinking, so please don't take any offense at my writing style

JWood · 2008-01-19 14:47

simon,
sorry for my delay.
·
I just posted an updated version of the IMU code on object exchange. Download that and give it a try.
·
Your YRate should be like your XRate. So depending on how your mount the 5DOF circuit you may need to fix that solder joint.
·
All your raw measurements from the ADC look correct (except YRate). So I'm hopping this new code helps you out.
·
be sure you don't reverse the CS, DIO, CLK values when you pass them to the object. I've done that a few times and you get some crazy readings as you can imagine.

simonl · 2008-01-19 16:32

Hi Jason,

Thanks for checking my data.

I see the new code's not yet made it to obex, so I'll have another look later.

Many thanks.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,

Simon

www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
BTW: I type as I'm thinking, so please don't take any offense at my writing style

simonl · 2008-01-19 16:38

Ahh! I see the 'published' date doesn't get updated on obex. I've downloaded it now and will try it this evening.

Thanks again

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,

Simon

www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
BTW: I type as I'm thinking, so please don't take any offense at my writing style

simonl · 2008-01-21 10:26

Hi Jason,

I'm still getting something wrong here I'm afraid :-(

I've fixed the yRate problem - by shortening wires! My set-up is now as follows:

+               +----°-----+
¦- X Accl ---- 0¦1       16¦---> +3.3V
¦- Y Accl ---- 1¦2       15¦-+
¦- Z Accl ---- 2¦3       14¦---> GND
¦- ST -------- 3¦4       13¦---> CLK:PIN -> P27
¦- Volt Ref -- 4¦5       12¦---> DIO:PIN -> P26
¦- Y Rate ---- 5¦6       11¦-+
¦- X Rate ---- 6¦7       10¦---> CS:PIN  -> P25
¦- GND ------- 7¦8        9¦---> GND
¦- 3.3V         +----------+    
+

As a result I've changed the pin assignment variables:

xAxis = 0
yAxis = 1
zAxis = 2
VRef = 4
yRate = 5
xRate = 6
supply = 7

...and I've changed the MCP mode to %0111_0111.

My set-up also uses a 10MHz Xtal, so I've set '_clkmode = xtal2 + pll8x' and '_xinfreq = 10_000_000', and this is all sitting on a breadboard (see photo).

I may be making some assumptions!

1. 'get_angle' emits current (estimated) angle of IMU in decimal degrees, min=0, max=359.99?

2. No idea what 'get_q_bias' and 'get_rate' emit!

3. IMU.spin keeps constant 'watch' on the IMU angle, and doesn't care when it's PUB's are called. (BTW: How often is it updating?)

What I'm actually getting - with the IMU level:

            ANGLE        BIAS        RATE
min        1109329237    -1098241873    -1110939232
average    1109471799    -1090719884    46344362.87
max        1109565419    -1067645163    1076858803

Not exactly what I was expecting LOL!

I have a feeling the issue may be in the following variables with respect to my set-up:

  dt = 0.0021452  
  R_angle = 0.3
  Q_angle = 0.001
  Q_gyro = 0.003

...but I have no idea what the value ranges for these are - any guidance would be helpful here!

Anyway; I'm sure it's something I'm doing wrong but, like you at the beginning, I think I've been looking at it too long to see my mistake. Any help you'd be happy to provide would be most welcome

I've attached my code and example data in the hope that it might help.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,

Simon

www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
BTW: I type as I'm thinking, so please don't take any offense at my writing style

JWood · 2008-01-21 17:17

Hello Simon,
·
I believe I see what your problem is. It’s related to both Hardware and Software.
·
Hardware Problems:
You have PEN 7 hooked up to ground … not supply. This will cause every Axis to be way out of normal scope.
·
Loop at this line in the code just before the main loop.
Axis0 := tiltReadings[noparse][[/noparse]supply] / 2
·
You see I’m basically getting 2048 at this line. Supply should be 4096 so ½ that would be 2048. Each AXIS of the 5DOF circuit has +-3 g forces and 0 g’s would be ½ the supply so -1 g would be (½ supply) - (½ supply)/3=1365· and 1 g would be (½ supply) + (½ supply)/3 = 2731
·
According to the mounting of your circuit from your picture you should be getting ADC readings like this.
X = 2048 (0 g)
Y = 2048 (0 g)
Z = 2731 (1 g)
·
So you’ll either need to change this line of code
Axis0 := tiltReadings[noparse][[/noparse]supply] / 2
To be
Axis0 := 2048
Or fix your setup so pen 7 is supply.
·
Software Problems:
All the return types of all the public functions in that Object are Floats.
·
You are sending the data serially using this line: tx.dec( angl )
You see that you’re sending the data as a decimal not a float. That will give you some very crazy numbers back. Such as (1109329237····-1098241873····-1110939232) like what you had seen. I suggest using the FloatString object and convert the Floats to a string and pass that serially instead.
·
tx.str( fString.FloatToString(angl) )
·
You’re Assumptions:
1: 'get_angle' emits current (estimated) angle of IMU in decimal degrees, min=0, max=359.99?):
is almost correct. It does emit the estimated angle but it’s a float.
2: No idea what 'get_q_bias' and 'get_rate' emit!
Get_rate is very important. It’s the amount of turn in degrees/second along the axis. And Q_Bias is the amount of “walk” the gyro has done from the start till now. A Gyro walks away from it’s starting position over time.
3: IMU.spin keeps constant 'watch' on the IMU angle, and doesn't care when it's PUB's are called.
··········· Yes it starts a COG and runs its code maintaining the 3 return variables: Angle, Rate, Q_Bias
I have the object update the state every loop and updated the Axis angle ever 5^th loop. This happens as fast as the COG is running and can crunch through the Float Math.
·
Being that your running 10Mhz Xtal I’m not sure if that will affect this objects timing. It took some tweaking to get the best dt time but at 80mhz clock and 5mhz xtal I found 0.0021452 to be the magic number.
·
As for
·
··R_angle·=·0.3
··Q_angle·=·0.001
··Q_gyro·=·0.003
·
They are variables to tweak the kalman filter.
·
R_angle is the amount of rad jitter expected from the accelerometers.
·
Q_angle and Q_gyro are weight variables to describe how much you trust the accelerometer angle over the gyro output rate. They are good where they are so you don’t need to worry about it.
·
I hope this helps.
·
let us know if this works and your outcomes.
·
Good Day
Jason
·· Wood·

simonl · 2008-01-21 18:15

Hi Jason,

Many thanks for the quick response

See - I knew it was me

I'll try again this evening.

Thanks again.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Cheers,

Simon

www.norfolkhelicopterclub.co.uk
You'll always have as many take-offs as landings, the trick is to be sure you can take-off again ;-)
BTW: I type as I'm thinking, so please don't take any offense at my writing style

Kalman Filter (Working) Small bug

Comments