Kalman filter test for sensor fusion (GPS + accelerometer)

Kalman filtering tutorial
• Kalman Filter Tutorial
Greg Czerniak's Website
greg.czerniak.info/guides/kalm...
Kalman Filter For Dummies
bilgin.esme.org/BitsBytes/Kalm...
Source code:
www.iforce2d.net/iforce2d_kalm...

[@sirdimka]

Well done! Very impressive. Looking forward to the next parts of the video

[@kalelsoffspring]

This is a very interesting way to simulate the real world, and a cool method for "sensor fusion". Looks awesome

[@arnzzz1]

Love this series about quads :)

[@robwasab]

"Once you realize that you have to get something done... well you just sit down and figure it out" - not quoted exactly. True...

[@petermorris1769]

Interesting video! Is there any chance of seeing some source code for the Kalman filtering?

[@edwardwillison2087]

This is really great, thank you!

[@keepeetron]

I was thinking it'd be cool (but probably pointless) to have an actual mini box2d accelerometer on-board the quadcopter using tiny pendulums (or however they work). nice videos by the way, i'm out of my depth here but this stuff is really interesting and you explain it well.

[@robosergTV]

this is so awesome. Thanks.

[@zes7215]

no such thing as storix or mentl blx or not, think/can think anyx nmw and any can b perfx. any happenx nmw

[@pfever]

This is awesome ,thank you very much for the video! did you happen to make this open source? Would be awesome to understand kalman filter better

[@KX36]

Something I picked out of that TED talk (IIRC) was that it plans the route, performs 1/20th of the route then replans it. Seems like an interesting implementation. I expect a well tuned PID controller combining GPS and accelerometer would be able to perform very well, get there in the minimum time, minimise overshoot etc. I have tuned PID control circuit for an electronic switching power supply, but that was all analog and I found that I could tune it very to transition from one specific start point and end point fast and without overshoot, but changing the parameters requires a different tuning. I have also used PI control for the same purpose. I found it to react much more uniformly to different parameters but always overshoot as without the D it won't slow down until it actually reaches the target. I watched the ControlLectures youtube channel for the basics of what PID is but for actual implementation I'm just learning as I go.

[@iforce2d]

The system used in the TED talk is quite different to GPS. They are using infra-red reflective markers which allows them to get the full 3d position and orientation of the quad to about 1cm or so of accuracy, over 100 times per second. That allows them to do some truly amazing stuff. A good GPS module (at least the commonly affordable, non-military kind) will refresh at 10 times per second and may be off by many meters, that's why it takes time to gather more readings and get a better picture of where it is, and will probably not be able to perform much better than this. If you watch videos here on KZbin of quadcopters doing return-to-home, they usually do it at a pretty leisurely pace :)

[@RobertShaverOfAustin]

***** Many years ago I worked on the hardware/software for a military drone project. This was before GPS was fully deployed, so we used the worldwide Omega navigation system. en.wikipedia.org/wiki/Omega_(navigation_system) It was less accurate than that GPS is today, but we used something called differential omega. In this system we places a stationary ground Omega station in the same area in which the drone would fly. This differential station would watch it's own Omega position change do to atmospheric events and transmit a change signal to the drone. The nav system in the drone used the received delta position to modify the position from the on-board Omega nav system. This Wikipedia article *Differential GPS* en.wikipedia.org/wiki/Differential_GPS says that 10CM accuracy can be achieved. Perhaps you could build the differential base station into your transmitter and transmit the differential adjustment value along with the digital control date from your joysticks. Just a thought. ADDITION: Looks like I'm late with this suggestion: diydrones.com/forum/topic/listForTag?tag=DGPS

[@iforce2d]

Robert Shaver Right - I had been told about that by some folks earlier. I think the major issue is that the updates are only at 5Hz, which is slow compared to how quickly the vehicle can move, rather than the absolute accuracy. In reality the GPS accuracy is not nearly as bad as I have made it look in this video to test the algorithm. Check out my "follow me" video to see two GPS modules working together at 5Hz, one directing an RC car to follow the other. It's not all that bad.

[@ShirishJadav162]

how is this simulation made ? I want to implement similar simulation but I failed to make copter simulation as I was simulation power on individual motors

[@julienpfefferkorn3454]

great video. one comment though: I doubt that a vertical accuracy of 3m is realistic for a GPS. in my opinion it is rather in the order of magnitude of 50m. I can imagine that an accurate control would require to use also a barometric sensor on top of the GPS.

[@iforce2d]

+Julien Pfefferkorn yes, for altitude a barometer value would be used

[@gideonbuckwalter4128]

Hey, loving your series on quad design! For a while now I've been wanting to get the hang of PID controllers and the like. Do you know of any good resources for learning automated control algorithms? Thanks

[@iforce2d]

Sure, this guy comes to mind right away: kzbin.info The PID I'm using actually comes directly from the pseudo-code on the wikipedia page. You might also be interested in this video, which has a link in description to my blog which has some source code: kzbin.info/www/bejne/gnmToYV6fsqNeJo

[@gideonbuckwalter4128]

Great, thanks!

[@RobertShaverOfAustin]

Lovely simulation you've got there. I'm interested in knowing more about your simulation environment. From my perusal of your other videos it appears to be based on Box2D. I have the following questions: 1. Are you willing to share your code? 2. What parameters are you simulating? (copter mass, battery power, motor thrust, GPS position with error, accelerometer readings, throttle setting) 3. Are you simulating accelerometer error? 4. Are you simulating any latency? I'm thinking of the time between readings in a real flight controller. I wonder how far you could get with position-hold using just accelerometer readings? In a no-wind situation it seems like the perfect accelerometers could keep the copter stationary. Do you have any insight into the precision, sensitivity and lag present in the commonly used accelerometers used? That's for a great channel and some interesting projects.

[@iforce2d]

I might share the code later once it's working more solidly and settled down. Well, the whole thing is running in a computer so every part of it is being simulated... not quite sure what you're getting at. To make things somewhat more realistic I'm throwing in a little randomization in many places, eg. the applied thrust values for the motors, the readings from the 'accelerometer' etc and I'm making the 'motors' take a tiny bit of time to change speed. Apart from the GPS readings, I'm not anticipating latency to be much of a problem. The radio link should be able to handle about 100 packets per second, and the accelerometers can update about that fast as well. If you have a modern smart-phone you can find some apps that let you see all the raw data, to get a pretty good idea of what to expect from a consumer-grade accelerometer. Take a look in the App store or Play store for apps called 'sensor data' or something like that. They are very good, but I doubt they're good enough to do position hold on their own, even in a wind-less room.

[@neddolphin]

Nice demo! Would you be willing to share the mathematical formulation? I am particularly struggling with how to deal with (i.e. how to form my matrices) the fact that my gps refreshes once per second while my accelerometer gives me as high as 800 samples per second. Thanks!

[@iforce2d]

+deepblundon it's basically the same as the 'cannonball in flight' example from the second link in the description, except that instead of a constant downward gravity vector you'd use the accelerometer value. About different update rates, I never did get a great solution for that, in fact I've been reading up on it since your comment in the hope that I could give a better answer than this. But in the meantime, I found quite a few good papers by searching google for "kalman different update rate".

[@nilsschonherr2075]

Hi, did you try to implement this filter to cleanflight or some other flightcontroller, or did make an assisted GPS project, so one can use it, to get precise positioning? Really interested in the project! Did some testing in MatLab and results were ok, but had no luck, with building it with real sensors and so on...

[@iforce2d]

+Nils Schönherr I'm thinking a lot about that actually, but I can't use the matrix library that I did in this video which makes things very easy. I tried compiling cleanflight as C++ so I could use the library, but it didn't even connect to the configurator :( Besides that, the program size explodes when you start using the templated matrix functions. I'm afraid all the matrix code will have to be done by hand in C for the typical STM32 to run it, which has put me off for the time being (and I checked the Ardupilot code for it which also put me off :) I'm thinking about setting up a situation where the onboard flight controller is just a 'dumb' terminal that relays sensor data to the ground where a big boys CPU can work on it. This would allow development and testing to progress at a hugely increased pace, and perhaps the problem of getting it all done onboard can wait for another time. It would also require a location where I can fly a quad outdoors yet still within range of my desktop computer, a situation I am also working on...

[@programagor]

+iforce2d What about using Raspberry Pi Zero as an onboard computer?

[@rickvs87]

Thx for the demo! Very helpful in visualising the quadcopter ;) But i have a question regarding the GPS. When i leave the gps static in one location after a little while i get this drift, which increases to a couples of meters in minutes. This is not a issue while moving, but when trying to implement a position hold, it quite annoying. I did some googling on the matter and it seems to inherent to gps. In your simulation you dont the this drift into account when hovering, but i was wondering if you found any solution for this?

[@iforce2d]

I think the only solution to that is to buy a better GPS or improve the antenna/reception of the one you have.

[@rickvs87]

Hi, thx for your reply! I have an ublox m8n (from dealExtreme) so i thought a had a "good" gps. Which is this lib your using for your matrix math and were you able to use this on a arduino?

[@iforce2d]

I'm using a lib called eigen, it is easy to use but is template based. There is no chance it will run on a regular arduino. I have managed to build and run a very small test for STM32F403RE though.

[@SohailKhan-zb5td]

Very helpful video, what application your using for simulator.

[@iforce2d]

This is called Box2D.

[@mattiasfagerlund]

I love these - why haven't you fixed R.U.B.E. so you can create these experiments within the tool? That would be awesome!

[@iforce2d]

hey, hows it going :) These are a bit specialized to somehow make a generic editor for :) Eventually I want to add scripting into the player view as well, which might let you do some basic prototyping of behavior though.

[@mattiasfagerlund]

***** I can see how this might be too specialized. Now you've got me reading about Kalman filters, interesting stuff. Gotta bone up on PIDs as well, for no other reason than that they're interesting stuff. Hope to see a video of this one in action on a read quad soon - hopefully not on a windy day though ;)

[@iforce2d]

Mattias Fagerlund I did some more work on this later (the guy Greg mentioned in the video description gave me some tips about things I was doing wrong) and after playing with the PID settings some more I have managed to get the wind problem sorted out :)

[@AmanKumar-ng9vg]

Which software you used for data visualization?

[@iforce2d]

box2d testbed. In other words, just raw opengl :)

[@numenical]

Nice simulation. Just a word of caution; in the real world, accelerometers see 1g from gravity, which you don't appear to be simulating. Fortunately Kalmans can account for this by including a gravity term in the dynamic model.

[@iforce2d]

+JimTheGuyWithTheBrain aah, I see you are a guy with a brain. Yes, I was warned about that by the guy who wrote one of the tutorials I linked to, I think it can be worked into the real calculations without too much trouble when the time comes.

[@saswatibhattacharjee7387]

Nice video. Just one request that can you provide me the raw data of Accelemeter and GPS. I have a project where I use it.

[@iforce2d]

As I said in the video this is just a simulation, where I made up some fake values. There is no GPS and no accelerometer. Maybe you can find some log files for flight controllers like Ardupilot which would be useful.

[@countrycountry88]

which tool do you use to simulate the copter graphically?

[@iforce2d]

+countrycountry88 it's Box2D, a 2d physics engine used in computer games.

[@wildbill4680]

Hi, the flowing sensor claims to have inertial and GPS navigation via kalman filters. It is quite expensive compared to standard M8N... what are your thoughts on this part please? If added to a UAV would it give improved accuracy above and beyond a standard M8N or is serious programming required to take advantage of its features? surly some of this could be done from the sensors on a UAV with a standard M8N? or is the clever stuff done on board this unit and therefore its worth the price? BR-126 GNSS/MEMS GPS Inertial Navigation Module BDS+GPS Dual System For Ublox NEO RC Drone www.banggood.com/BR-126-GNSSMEMS-GPS-Inertial-Integrated-Navigation-Module-BDSGPS-Dual-System-For-Ublox-NEO-p-1193795.html?rmmds=mywishlist&cur_warehouse=CN thanks,

[@kerron_]

What software is that

[@hamoodelholandy]

can u give me a links or eny resources that learns how i can combine the data from the GPS and the IMU data with Kalman for localization and navigation Rover

[@iforce2d]

+Hamood Hamood check the links in the description

[@hamoodelholandy]

+iforce2d yas about Kalman filter theory i understand it but i want to know eny method that compine it on Arduino if u can show mee eny resources that explaining IMU and GPS

[@iforce2d]

+Hamood Hamood what difference does it make that it's on arduino or not.

[@hamoodelholandy]

+iforce2d okay i get it can i chat with u in facebook or gmail to ask u about some qustions

[@iforce2d]

+Hamood Hamood we're already chatting here. it sounds like you want some source code to just paste and run, which I don't have.

[@DjekAmir]

the source code please !

[@ProjectPhysX]

Very interesting video! I have programmed a similar simulation in 3D. Check out my channel for more information.In the top left corner in your video there is a wind option. How do you model wind? Do you use random changing vector fields? Or is it jist fixed strength and fixed direction?

[@iforce2d]

Just a small random force added each time step, eg. ( rnd(0.9,1.1), rnd(-0.1,0.1) )

[@ProjectPhysX]

This would just create random noise because the time steps are so small that it wouldnt actually cause the quad to move. I ran into that problem too and i dont quite know an elegant way to solve it. You would require a random but steady function, like x=sin(t), when x is the x-coordinate and t is the time in seconds. However the sin() function is not random and also very predictable, hence not useful for simulating wind.

[@iforce2d]

I edited my comment after the first post, perhaps you missed the edit - in my first post I did indeed make it a random noise. But in case it wasn't clear from my edited pseudocode, the x component of the force is given a random value between 0.9 and 1.1 which is on average 1. This would push the body to the right. The y component will average out to be 0 in the long term, but in practice it will move up and down a little (brownian motion in vertical axis).

[@ProjectPhysX]

Ok I got it. So you made the wind go in a particular direction with randomly varying intensity.