Angle measurement for balancing robot.

[HenrikOlsson]

Hi,
(Long winded story...)
I'm working on a two wheel inverted pendulum balancing robot. (photo attached). The basic idea is to sense the angle of the robot relative to gravity and drive the wheels so that they are right under the robots center of gravity.

To do this I need to meassure the tilt-angle of the robot and for this I have an ADXL203 dual axis accelerometer (only one axis in use). Now this would work perfect if it wasn't for the fact the accelerometer measures, eeh...well acceleration.

As soon as the robot accelerates to compensate as it starts to fall over the reading from the accelerometer is no longer only proportional to the robots tilt-angle.

In comes the gyro....an ADXRS150 in my case. This is a rate gyro, ie it
outputs a voltage proportional to how fast it's turning. If this voltage is integrated over time it's proportional to the difference in tilt-angle compared to the angle at power-on. In a perfect world, that is. The problem here, as you probably know, is that the gyro's bias is drifting pretty fast so the 'angle' value isn't accurate for very long.

Now, in researching other inverted pendulum robots, (like the nBot, bBot, gyroBot, etc) they all seem to 'merge' the readings from the acceleromter and the gyro via either a complementary filter or a Kalman filter. I have found code (GNU) from a heli auto pilot project for a Kalman filter
but the math is a bit daunting and it's written in C (whatever that is... :-)... just kidding).

Anyway, have anybody of you done anything like this with PBP and/or is willing to have a look at/work with me on porting the Kalman code to PBP (if possible). Or do you have any other ideas on how to acomplish this.
(Except for buing a FAS-G inclinometer for $795, that is)

Sorry for the long post, just wanted to show that I've done my homework ;-)
I do have a short video of it actually balancing (briefly) but I can't seem to upload it here.

TIA
/Henrik Olsson.

[SteveB]

I don't know if it helps, but I seem to remember someone using "range finding" as a technique. They used two sonar modules to measure the distance from the top of the robot to the the floor, then did some less arduious math to compute the angle. I can't remember if this was a "concept" or it was actually implemented successfully.

Cool project, BTW. Good Luck,
SteveB

[vacpress]

Nuts N' Volts magazine has recently run 2 articles about this exact topic. They used picc i think, but the concepts are close.. Their robot used hobby servos, sumobot wheels, salvo RTOS, and some fairly simple looking code. If you cannot find it, i can tell you issue #s when i am at home..

basically their bot used just a 2-axis gyro, if i recall, and an AVR.. not a pic.. no PICC.. Hmm.. At any rate, you can checkout their code, it probably avoids Floating point math, which will help you.

There is another method out there that peopel seem to actually have some success using; which is to use 2 IR range sensors pointed at the ground... Seems much slower than a gyro\accelerometer.. but, what do i know..

hope that helps at least a little.
-R

[sayzer]

I suggest that you use a third wheel.

You will not need to write complex coding and save a lot of coding space and parts.

:) :D

[HenrikOlsson]

Hi,
I've seen the IR-approach, there's a really cool working system here: www.balbot.com The thing is I'm not sure of the idea since the surface has to be flat - or atleast that's what I think. You can't run up or down a slope for example since the difference between the front and back sensor will be different. The same drawbacks would apply to the ultrasonic transducer approach. Besides, I already got the gyro and accelerometer.... ;-)

I seem to recall someone mentioning the NV article a while back. I may even have seen it and if I have I think they used the same kalman filter code I'm refering to but I may very well be wrong on that point.

I've been working on this on and off for the last 7-8 months, so I'm not gonna give up and put in that third wheel just yet but thanks anyway sayzer ;-)

Thanks guys! Please keep the ideas comming!

/Henrik Olsson.

[Darrel Taylor]

Hi Henrik,

Don't know if this will help or not, but one of our members, Tom Igoe, has a routine that's supposedly "based" on a kalman filter.

http://www.tigoe.net/pcomp/code/archives/arduino/000710.shtml

Versions in C and picbasic.

Don't know enough about it to know if it's what you're looking for.

HTH,

[arniepj]

Using a inclinometer or clinometer is another option,though it might be expensive.I have used the Accustar(data sheet attached)with a serial connection to a Pic,but not for a balancing act.If you google clinometer you might find something useful.

[HenrikOlsson]

Thank you for the link, Darrel. I'll read the code and the .pdf Tom says he based it on and see if may work. I'm not sure in what way it resembles the Kalman filter. Since I don't really understand how the Kalman filter works that may or may not make any difference.... ;-)

Arnie, thanks for the sugestion. However, reading the datasheet for the Accustar I came to the conclusion (from the description of how it works and please do correct me if I'm wrong here) that it will suffer from the same problem as the acceleromter. It would work when it's stationary and rotated around its own axis but as soon as it is exposed to a "another" acceleration (except gravity) (robot starts to move) the reading will no longer reflect the true angle of the platform.

The only one I've found that would work is the FAS-G from Microstrain (which contains an acceleromter, gyro and uC) but it's WAY to expensive.

Please keep the suggestions coming. I have some more reading to do.

Thanks!

/Henrik Olsson.

[HenrikOlsson]

Hi again,
I read Tom's code and the pdf he based it on. Turns out that his code is not based on the Kalman filter but a "simpler" weighted average filter which is actually what I'm using. The pdf shows the code for the Kalman filter but it is again in C which doesn't make it very easy for me.....

What's that saying, it's the last 10% of the project that takes 90% of the time - the devil certanly IS in the details.

/Henrik Olsson.

[Luciano]

Hi,

See the PDF file below.

Adjustable weight position, shock resistant.
http://img4.picsplace.to/img4/29/joe_p.JPG
Swiss Made! (This is a special note for Alain).

PDF doc:
http://leiwww.epfl.ch/publications/grasser_darrigo_colombi_rufer_mic_01.pdf

See the movies:
http://leiwww.epfl.ch/joe/

Best regards,

Luciano

[HenrikOlsson]

Hi Luciano,
Yep, seen that one before too, very cool and seems to work extremely well. The problem again is that the math described in that pdf is so over my head that I can't even begin to understand it. I was hoping that someone (with enough math skills) already had wrote a routine or ported one of the available routines to PBP.

Thanks!
/Henrik Olsson.

[Ioannis]

Just out of curiosity...
How will the robot rise if it is in a initial horizontal position? Alone I think, will not!

Anyway, JOE is very impressive!

The PBP in my opinion is a little slow for the required math.

Ioannis

[sayzer]

Just out of curiosity...
How will the robot rise if it is in a initial horizontal position? Alone I think, will not!
....

A second one will help it to stand up of course!
Like a for loop.

IF the second one is also at initial position THEN NEXT.

---------------------

[Ioannis]

Aha! Good idea Sayzer!

Ioannis

[HenrikOlsson]

Ioannis,
I don't think Joe can stand from a horisontal position on it's own. If I ever get mine to work it will hopefully be something like this one:

http://www-robotics.cs.umass.edu/Research/Distributed_Control/uBot/uBot.html

In one of the videos they show how it recovers from its "resting" position. Very cool and stable two wheeler.

The math seems to range from rather complicated to extremly complicated. Since my mathskills are rather (very, in this case) limited I'm aming for complicated at the most. I'm confident it CAN be done with PBP i just have to understand HOW it works.

I have quadrature encoders mounted on the wheels and I'm thinking I can calculate the robots acceleration by comparing current speed with previous speed and subtract that from the acceleromter reading ending up with 'pure' tilt. My concern now is that the resolution of the encoders are to low. But hopefully I'll be able to understand this Kalman thing any year now..... ;-/

Atleast it's a fun project :-)

/Henrik Olsson.

[ronf]

I don't know if anyone has investigated this one
http://www.circuit-ed.com/accel.htm
Might be worth a try for the angle part and not toooo much $.

[vacpress]

soo..from reading the nuts n volts article, i think the PBP is fast enough. After all, PBP gets compled into ASM, even bloated ASM will run pretty well at 40mhz with only a few tasks, right? this is the advantage of picbasic over parallax..

the nuts n volts december 2006 article pretty much spells it out.. in c though.. they use a RTOS as well.. real time operating system.. this is a type of OS with direct control of hardware. windows is not RTOS, linux is, for example..

the october 2006 article, which i cannot find covers the math, probably in pretty easy to understand terms.. i suggest you look for that... otherwise why not teach yourself the mathematics you need? how hard can it be? brak it into bits... find the main equation and do your best to understand it, then look at some code... the code will look much different...

ive never tried this type of project, but there are a ton of resources.. i watched a video of some really stable 2-wheelers.. off roading, over curbs, etc... also a relaly cute one that could follow children wearing red and had very compelling movement characteristics due to the PID correction being a bit coarse... it dipped and swayed with the child...





anyways... they list these resources:

www.geology.csmu.edu/~dpa-www/robo/nbot

http://autopilot.sourceforge.net/gallery.html

barello.net/Robots/gyrobot/index.htm

jormundgand.net/projects/crunch/

sparfkun.com

--------------------

[HenrikOlsson]

Ronf,
Thank you for that link, good price on that one! But it is a acceleromter so it won't work (on it's own) for sensing tilt in a dynamic enviroment. I have the sensor hardware covered, accelerometer and gyro, it's how to combine the two readings into one stable tilt angle that is the problem/exercise.

Vacpress,
Thank you! The Kalman code that I posted IS from the NV article. They in turn probably got it from the AutoPilot project on Sourcforge, it's written by Trammell Hudson at Rotomotion. They supposedly used to make low cost IMU's but seems to have moved on to bigger high dollar things.

It was indeed the nBot that got me started on this. I have since looked at everything I could find on the net.

What you say about learning the math is what I'm trying to do here ;-) If someone already had ported it PBP it would be easier (relative term) to understand it than from the C code from Rotomotion.

It seems that some get away by just integrating the rate gyro signal but that doesn't work to good with my sensor, which is the ADXRS150 that I got from www.sparkfun.com

I took the same route as the nBot creator and started by balancing a pole on top of a stable (non tilting) platform. So I have the PID code for driving the motors once I have stable tilt angle feeding the PID regulator.

This is a complex project, motivation and time comes and goes but i WILL make it work, someday.... ;-)

Thanks guys!
/Henrik Olsson.

[Luciano]

Hi Henrik,

Why not work first on a simplified version of
the self-balancing robot?

==========================================
Version 1: (For flat surfaces only / indoor).

Use a Sharp infrared sensors to measure the distance to
the floor and uses that information to deduce tilt angle.
==========================================

==========================================
Version 2: (For flat and inclined surfaces).

Encoders + rate gyro
==========================================

Try the second version of the self-balancing robot only when
version one works as expected. I am sure that you will
able to re-use part of the code and experience developed
during the realization of the first version.

Best regards,

Luciano

[HenrikOlsson]

Luciano,
Absolutely!! 2 Sharp GP2D120 ordered from ARRobotics, hopefully they'll be here within a week or so. If (when) I get that to work it will boost my motivation even further.

Thanks!
/Henrik Olsson.

[uffi]

Hi Henrik,

I am also working on a balancing robot. My platform is an ATMEGA16 with L293D motor controller ICs and I am using a two-axis accelerometer board built with Freescale MMA2260DR2 for x-axis and MMA1260DR2 for z-axis with a bandwidth of 50Hz, which should be fast enough for a pure accelerometer solution. The robot's mechanical structure is built with Fischertechnik components. I also use a rate gyro CRS03-02 by silicon sensors (bandwidth 10 Hz, which could be a little too slow).
I also asked myself whether it could be possible to use only the accelerometer alone or the rate gyro alone. To solve the problem of the acceleration of the robot influencing the measurement values of the two-axis accelerometer I would like to suggest the following to you:

First of all I would recommend to use both readings of the two-axis accelerometer. The accelerometer should be placed in the height of the motors. If you use the x-axis in direction of the robots acceleration and the z-axis in the direction to the sky or ground (starting initially without tilt), both the tilt and the acceleration can be calculated from the data using trigonometric relations and solving a squared equation. I used the drawing in the paper of Joe Le Pendulum for reference, see attached file Tilt_Angle.doc or their paper on the web, Figure 5.

Then the forward (or backward) acceleration a of the robot can be calculated to:

a= +- SqareRoot( Square(x) + Square(z) - 1)
+ for the case a>0
- for the case a<0

and

tilt=arcsin((x-z*SquareRoot(Square(x)+Square(z)-1))/(Square(x)+Square(z)))
# for the case a>0

tilt=arcsin((x+z*SquareRoot(Square(x)+Square(z)-1))/(Square(x)+Square(z)))
# for the case a<0

tilt=arcsin(x)
# for the case a=0

All values are normalized to the earth's acceleration g (g=1).

The remaining unknown figure is the sign or direction of the acceleration a to get the correct result for the tilt. This could be derived from the motor control comparing the motor power during the last two control steps before this measurement.

For the motor control it is then essential to use in addition to the tilt also the tilt change rate (for a PD regulation) by comparing the last two tilt values (differentiating), because if you only use the tilt itself, the reaction of the motors will be too slow.

If you only want to use the rate gyro alone (as was done for Joe Le Pendulum), then you need a way to compensate for the gyro's drift. A simple way of doing this could be to work with a running average for the offset. Make a reference measurement of the offset as a starting point and keep the robot in an upright position for a short time after releasing the reset button. Then update the offset with a weighted value of the rate measurement, such as:

offset_new= offset*(999/1000)+ rate/1000

for every new measurement of the rate. The weighting should be adapted to the timing period of your measurement updates. By this the drift of the gyro could be compensated. I am working on this tuning at the moment. Assuming a measurement timing period of 10ms (100Hz), the above equation would correspond to a low pass filter with 100Hz/1000=0,1 Hz filtering the dc component of the measurement signal to be used for the offset.

All this I am writing here is still in an experimental state and are thoughts I developed during my research over the last weeks. I hope you can make use of these ideas. If so, please let me know. My own robot is not yet perfectly balancing, I am tuning the PD regulation constants and the offset filtering at the moment based on the pure gyro rate approach. Next step I will do then is use the pure accelerometer approach.

Kind regards, uffi.

[HenrikOlsson]

Uffi,
That is some nice info, THANK YOU! I will definetly investigate and/or try both methods and will post any results in this thread. Please let me know how you are progressing as well!

Thanks again!

/Henrik Olsson.

[uffi]

Henrik,

one question regarding your posted attachment "kalman.zip". In the source "tilt.c" a header file "parameter.h" is linked, which is not included in your file "kalman.zip". Also I could not find these files in the latest releases of the autopilot UAV command and control project (version 2.5, 2.4 or 2.3). Which project at sourceforge has developed these files?
Can you post also the file parameter.h?

Thanks in advance, uffi.

[HenrikOlsson]

Hi Uffi,
I think that the original files came from the UAV project on sourceforge. Anyway the zipped archive of the complete file-set as found on the Nuts & Volts FTP-site is 345k and the max attachement size allowed here is 200k. So I'll attach the Parameter.h only (change file extension from .txt) and if you want the rest I can e-mail it to you directly or you can get them from the N&V FTP-site. Here: http://www.nutsvolts.com/~downloads/B-Bot.zip

Are you saying that you are working on converting the routines to PBP? If you do and want to share it I will owe you big time!

I just got my two SHARP GP2D120 in today so I will have a go at using them first. Then I'll revisit the gyro/accellerometer approach.

Hope to here from you!

/Henrik Olsson.

[Archangel]

Thinking about balancing robots, a lot of energy is spent to do this, both electrical energy and your energy. I would suggest using large wheels and ballasting the robot below the axle to lower the C/G. Perhaps the batteries could go there. then install one of the gyros sold for R/C helicopters to help stabalize the bot. I think then your code for the motors would be less critical, and robot would would make you smile sooner (even if the ballast is inadequate to allow it to stand without controller operating, it should help).

[vacpress]

joe - this was my first reaction also. but then i read this N&V article... they put the batteries in the 'head'... and consider the segway... sure the batteries are in the base, but the center of gravity probably is closer to our mid section when it is being ridden...

i get the impression that a high center of gravity is important.. i also notice that most bots put the accelerometer low, near the wheel axle height...

if anyone has more info.. i am reading along.. interested in the concepts, if not actually making my own balancing robot!

i know it may sound like sacrilege, but try the parallax forums.. very helpful people there...

[mister_e]

It's 2007, resource are everywhere and mostely free... so what's the problem to look elsewhere, i don't see any. This is why divorces exist after all :D

In this case it's more about problem approach and/or problem analysis than anything else. Time to dig in you old school books or take a few cup of coffee to see what's happen ;)

[Archangel]

joe - this was my first reaction also. but then i read this N&V article... they put the batteries in the 'head'
I will take the time to read that article again.
JS
Edit:
The article says they moved the batteries up to "increase the inertia", I see that as an attempt to cause the accelerometers to respond faster. Their robot chassis is made of whiteboard, probably the reason they needed to increase the inertia, as that cardboard is not very stiff, I believe they could have achieved similar results by moving the accelerometers up to the "Head"
where nature put ours. Anyway those are my thoughts on the matter, It's up to you to sort out whats good and what's useless.
Have fun,
JS

[HenrikOlsson]

Hi Joe et all,

Try this: Take a pencil (low inertia) and try to balance it on the tip of your finger. Not really that easy is it? Now take a 1m (3foot) stick (more inertia) and try again, easier...?

Even better, take a pool que, try to balance it with the "fat" end down (batteries mountes low), then turn it around (batteries mounted high), which is easiest?

/Henrik Olsson.

[Archangel]

Because you are moving the part with the lower mass

[Ioannis]

That's right. It is easier to adjust something with lower mass than the Lead-Acid Batteries!

Also putting the accelerator sensors down, produces less spastic movements of the motors. Inertia of the highest parts takes care of the rest.

It is a very impresive project and needs good close loop control design. Although I don't know if it worths it since the energy to accomplish that is way to high.

But, on the other hand, for the wow's that the designer is going to accept, may be it is worth it!

Ioannis

[uffi]

Hi Henrik,

thanks for the file and the link. I downloaded all the stuff from the nuts 'n volts FTP server. Obviously these guys creating B-Bot have adapted some code originally developed for the helicopter autopilot on sourceforge. Thank you again.

Regarding my implementation: no, I am working on a C-code implementation for the ATMEGA16 to be compiled with win-avr or gcc, resp.

When you adapt my formulas for the tilt angle from accelerometer data, please care for the fact, that the accelerometers have different sign for dynamic and static acceleration. This fact is not yet considered in my formulas. So the sign of "a" must be inverted for the real measurements.

Have you done any work on implementing my formulas?

Kind regards, uffi.

[HenrikOlsson]

Hi Uffi,


Regarding my implementation: no, I am working on a C-code implementation for the ATMEGA16 to be compiled with win-avr or gcc, resp.

Oh crap, yet another C implementation....and not even for PIC... ;-) I've started to look at C for PIC's but that's not a topic for this forum.

Yes, the Kalman code comes from the UAV project. I have not had time to work on implementing your formulas yet but thanks for the pointer on the sign of the acceleromter readings. I'm not sure the accelerometer formulas will work with the sensor I got now. It is a two axis accelerometer, X and Y but I'm not sure how it will respond if mounted so it "becomes" X & Z. I will have to experiment with that too.

I got the SHARP sensors yesterday and managed to get the robot balancing fairly well. It's still a little "nervous" but I think I'll get it better by filtering the readings and/or experimenting with the sensors mounting. The goal, however, is the acceleromter/gyro aproach since the SHARP sensors introduce too many drawbacks IMO. But hey, atleast it works!

Thanks!
/Henrik Olsson.

[InitialDriveGTR]

If you need gyros, try the analod devices ADIS16250 or ADIS16255

[sayzer]

Henrik,

Do you need 180º or 360º for your angle measurement?

I "think" it should be 180º.

-----------------------------

[HenrikOlsson]

Hi,
I'd say +/-45 degrees from "down" will be more than enough. The problem, again, is that it needs (I'd like it) to be an absolute value in reference to gravity.

/Henrik Olsson.

[uffi]

Henrik,

during the last weekend I had a breakthrough using the kalman filter code you provided here from the nuts&volts article. I got my robot balancing based on combined gyro and accelerometer approach. Up to now I also used only one axis of my 2-axis accelerometer (therefore containing also an error due to the measurement of the robots acceleration itself: this error seems to be filtered away by the kalman code). The regulation is only PD-control, not PID, but I plan to extend it to PID to get better control of absolute position. I have a demonstration video (6MB) which is too large to be uploaded here. If you send me your email address, I will mail it to you.

All approaches based on using only the accelerometer or the gyro were not successful so far: the pure accelerometer approach has a too noisy signal to be used for this kind of control and for the pure gyro approach I was not yet able to really compensate for the drift only by software.

Have fun, Dirk.

[HenrikOlsson]

Hi Dirk,
That's absolutely great!
I haven't had time to work on mine for a while. I did get it to balance fairly well using the SHARP sensors though but nothing have been done to it since then.

So based on your experience (and mine, so far) I think that we can rule out using just an acceleromter or gyro. I kind of had that feeling from the begining but was I would be glad to be proven wrong.

Now, if we only could port that filter code to PBP. Hmmm...I wonder if there's a way to compile just the filter code and use the assembler code from within PBP.

Thanks for the update, I appreciate it!

/Henrik Olsson.

[Darrel Taylor]

Hi Henrik,

Just wondering if you used your new PID "Code Example" on the Balancing Robot?
<br>

[HenrikOlsson]

Hi Darrel,
Yes I did. The robot was the reason I wrote it. It a 18F2431 for each wheel running a PID compensated velocity servo at 1220Hz thanks to your interrupt routine. The main CPU runs the PID for the actual balancing at 100Hz.

Any chance you see some way to improove it?

/Henrik Olsson.

[Darrel Taylor]

>> Any chance you see some way to improove it?

Actually, yeah, I've got a couple ideas. I'll post them in the other thread.

But, back to the Balancing Act.
Did you use it in place of the Kalman filter, or with it?

Just curious.

[HenrikOlsson]

Hi Darrel,
Nope, the Kalman filter (if I ever understand/get that to work) would be used only to track the absolute angle of the robot platform. (Basicly removing the gyro drift). The error (desired angle - actual angle) is then fed to the PID-filter which in turn tells the motor-driver how fast the wheels needs to spin to keep it upright (or whatever).

As it stands at the moment (with the SHARP sensors) it just subtract the front sensor reading from the rear sensor reading and sends that value the PID-filter.

Thanks

/Henrik Olsson.

[uffi]

For those of you interested, I put here the link to my internet homepage, where you can find videos and description of my two-wheel balancing robot named ARTIST.

http://home.arcor.de/uffmann/ARTIST.htm

Have fun, Dirk.

[bbarney]

don't know if this is the answer but check it out
http://www.austriamicrosystems.com/03products/products_detail/AS5043/applications_AS5043.htm

[HenrikOlsson]

Hi,
Thanks! Unfortunatly it won't work. That sensor outputs a value based on how a magnet placed in front of the sensor is rotated.

/Henrik Olsson.

[bbarney]

I just ran across it by mistake and remembered your post and thought maybe it would work,some interesting stuff there though

[RodSTAR]

I suggest that you use a third wheel.

You will not need to write complex coding and save a lot of coding space and parts.

:) :D

LOL that's innovative !!

[RodSTAR]

Well,
interesting project, in fact i did that some years ago with stepper motors, hardware talking, you must combine both sensors accelerometer and gyro, aswell as obvious other sensors for not to mess with walls and other more complex obstacles. I applied kalman in C, in those times, I'll try to give myself a time to work in a PICBasic version to share it and adapt it to some new personal proyects I have in mind. Added also a digital compass to test gyro's Z response. Calibration and software compensation is the goal for this. Neural implementation for this, is the way to be taken.

Regards
Rodrigo,
RodSTAR

[HenrikOlsson]

Hi Rodrigo,
I haven't worked on the robot for several months. I did get it to balance a bit with the two SHARP sensors but the goal is to use the gyro and acceleromter. I beleive the Kalman filter is an esential part of the equation to get that to work correctly. If you ever get to porting any of your Kalman code to PBP I'd very much appreciate if you'd share it.

Thanks!
/Henrik Olsson.

[flipper_md]

Found this track about Kalman, and it looks quite a Maths challenge!

http://fred.unis.no/Kalman/Master_Thesis_Mathieu.pdf

very well detailed, but still complex.


this looks a bit more accessible: KalmanFilterforDummies
http://bilgin.esme.org/BitsBytes/KalmanFilterforDummies/tabid/196/Default.aspx

[Kamikaze47]

Sorry to drag up an old thread, but did anyone ever manage to get a Kalman filter implemented successfully in PBP?

I'm hoping to apply a Kalman filter to accelerometer and gyroscope readings to obtain a tilt measurement and I figured I'd ask if someone has done this in PBP before attempting to port the C code full of floats and matrix multiplication into PBP.

[HenrikOlsson]

Hi,
Not me.... But I'm sure it can be done with PBP, PBPL provides enough bits to get the resolution and but to be able to do it you need to actually understand how it works - which I don't :-(

If you understand the Kalman theory and/or how the C code works I'm sure that we, collectively, can work it out if we take it step by step.

I'd really like to see working Kalman filter in PBP as well.

/Henrik.

[Kamikaze47]

I've been researching it and i'm starting to understand it better. I can certainly follow the C code, but theres a lot of things in there that arent so easy in PBP, like floats, matrix manipulation and trig like arcsin.