Variation of an inertial navigation system

OK so I'm working with a 9deg of freedom IMU, a GPS from uBLOX, a serial LCD,SD card logger,a slew of other sensors, and Arduino MEGA of course running on a liPo. All circuits are running at 3.3VDC 8Mhz. With that said I have a kalman filter working great and the sampling of the IMU appears pretty stable.

A little background on what I'm trying to do. I'm looking to build an inertial navigation system but not in the sense of an auto pilot. I'm looking to plot Lat/log/Altitude for areas where my GPS can no longer give me data points. So basically I want to show my Log/Lat/Altitude on the LCD screen and when I loose the gps fix and altitude sensor to use the IMU to calculate the Lat/Log/Altitude from that point until i get back into GPS range. This needs to be done in 3D. Think of mapping the inside of a cave to help people with my project. If my error is 25ft or less after 1 hour I'd be very happy with the results. I would want to mount the unit on my chest or on my back to help cut down on noise but the unit will still be using attitude calculations do to the fact that you sometimes have to cram into little place in weird ways to get though things.

Any guidance would be great. Thanks for reading and helping. Are there any libraries out there for the Arduino that do something like this before I try to reinvent the wheel?

stealthtransam: ... If my error is 25ft or less after 1 hour I'd be very happy with the results. ...

Four days early for this post.

JimEli I didn't understand your reference to my post.

Perhaps JimEli is referring to April 1, although that would make him a day out. Achieving 25ft accuracy after 1 hour looks to me to be way beyond what is achievable using low-cost inertial sensors.

Thanks for clarification dc42. Like i said in my initial post I'm more looking at ways to take me sensors and show me distance traveled not the accuracy yet. I've seen some great examples done over sees but nothing basic on the arduino. Most of it is done on a PIC and they are not giving up their source code for the math. really right now I'm just trying to take the sensor data and say I've traveled 3 feet from the initial GPS coordinate in the x direction 1 ft in the y and 10 ft in the z. something like that. I cant seam to find anyone that has done this on the arudino with good documentation. I'm having a hard time grasping the math calculations and I'm more of a visual learner. This is why I made a post about it. If i could walk up and down the stairs in my house and it say I made it back to 0 ft x 0ft y 0ftz with some error that is expected this would be the stepping out I'm at right now.

What I think you basically need to do is:

  1. Have a calibration mode in which you hold the unit in a known orientation (e.g. upright and pointing North). In this mode you can measure the sensor outputs when the sensor is still so that you can correct for them later.

  2. Use the rotation sensors to track which way is up and which way is North. From this you can work out the transform for the corrected outputs of the linear acceleration sensors from the current orientation to the original orientation. Then you can integrate the transformed outputs to calculate distance.

However, I've never done this and I expect there are lots of tricks of the trade. Over longer times and distances, you need to correct for the Coreolis effect, for which you need to know your approximate latitude.

You could take a look at, although it's not quite what you want.

Very talented individuals from a variety of science disciplines have devoted their career if not life to design accurate systems for inertial navigation. High-end products are extremely complex in terms of a mechanical, electrical, math, modeling, design and manufacturing perspective. Some products rival the size of a refrigerator and cost more than a luxury home. These high end systems are typically custom made for a specific application and are not suitable for general purpose inertial navigation.

The basic principle of inertial navigation is fairly simple. Integrate readings from inertial sensors, add these to a known starting position and you have your predicted new position. The problem is that small errors accumulate over time and this is particularly difficult for a moving object such as an individual who can turn 180 degrees in less than a second and move freely in all three dimensions.

You could probably obtain some useful heading references from a magnetic sensor (such as from a mems based IMU) and you could add a barometric pressure sensor to track your altitude. Full 3D however is more than challenging.

Creating a hobby device based on low-cost mems sensors could be a good study project for someone with a passion for math and modeling, but to think the end result will be an accurate self-contained survey tool for cave mapping is unrealistic.

I think everyone is thinking that what i'm trying to do is mission critical. It's far from it. I'm not trying to put a rocket into space. It exactly what arduino was meant for a hobby. If I can over come the basic math I'm sure I'll be more then able to get my readings where I'm happy with them. I gave the cave mapping as an example not as an exact. I appreciate all the people telling me why I can't do it but I'm an engineering and to some degree I'm sure everyone on this forum is an engineer. This is an exercise in learning and education. Here are the basics in the steps I need to over come and what i'm needing to find out.

1) Filter the output of my sensor array to give a stable reading (done) 2) Non-3D linearly calculate movement in the x, y, z direction. (This is where I'm at and need guidance on the math) ex. Based on a starting point of 0,0,0 where all measurements are in FT. put the circuit board on the ground and slide it 3ft in the x direction. Have it tell me my new position is 3,0,0. (ideal conditions are expressed here only) 3) Determine attitude of measurement in respect to gravity. (easy to do) 4) Take liner measurements and integrate them into 3D space using trig and calculus as needed (not a problem here). 5) calculate long/latitude/altitude and display.

Step 2 is my big problem and the step that I am currently at.

Altitude/pressure is the easy one. I've got it down to 1ft until the weather system changes. You can determine which direction you're moving with a compass and accel. If it were a smooth movement you could integrate to find position. But not with a human jerking around. That's the problem. I've tried it for a motorcycle. Near impossible. Sorry!

How about measuring distance from a base unit with ultrasonic pings? Could go 50m? RF timing? Probably not Arduino...

thanks DC42 for the link. it looks like a good read.

To have any hope of working out movement you need to know the orientation of the device very accurately over the whole measurement time, as well as the acceleration in three axes over the same time in order to calculate the acceleration vector relative to earth. Inevitably, all these measurements will be inaccurate. Even very small inaccuracies in calculating the acceleration vector will cause a compounding error in the calculated speed and distance values.

Getting it to work accurately over timescales longer than a couple of minutes would not be practical imo unless you can find a way to detect and compensate for these cumulative errors.

You could perhaps account for these if you can make assumptions about the speed and orientation - for example you might be able to assume that when the acceleration is constant at 1G for a moment, the sensor has been put down so you can zero everything. You might be able to define limits for the maximum feasible speed in any dimension. You might be able to assume that the unit spends most of its time at a similar orientation and zero out any long term attitude changes, and assume that the most common direction of acceleration is 'up' and so on.

Perhaps, with a lot of work, you could get something that works ish.

How about a string with knots on it? A wheel odometer and compass? An RF based system similar to LORAN? I'm trying...

I've created a system which can determine the position of a flying object in space by measuring the timing of a laser rotating like a cone with a changing radius. In addition to pressure. Won't work in a cave. Limited distance.

I will be using this to determine precise orientation at small time intervals, not position or velocity:

I've discovered a way to measure the vector of a tiny change from "level" orientation using a cheap 2-axis compass module. Neither of these help with position. It is much easier when the module is of a stable orientation with smooth motion. Without this it is nearly impossible.

Here's a different method. If you had a map of the topography. For example a narrow cave with a slope. A parking garage that's not level. Or simply a hiking trail going up. Whenever you lost your GPS signal you could in theory determine your horizontal position based on the barometric pressure, from a given starting point and compass heading. For a short while at least. If the map were accurate. No?

This is interesting: It's easy to measure if you are not moving with inertia. To compensate for small movements with a motor. To keep a platform still in one position. But you cannot detect a slow drift without GPS or video (Brookstone Quadcopter). This is the opposite of the problem of tracking human motion while running. Here the error comes from sudden jerking movements instead. Seems that we have a problem at either extreme of stability, low or high. Somewhere in the middle it gets easier?

If you can't keep track of the horizontal motion of a helium balloon on a calm day as it is slowly released... It might be off by 10m in a minute. How can you possibly expect to do it with an object jerking back and forth on your back? Changing orientation constantly? We need to be more creative than the traditional method.

Ordered this: Will try to do something similar to what you wish, with much lower expectations.

Talk about low power!

I'm interested in this issue, because a friend is a caver, and complains of the difficulties of surveying with slackness amongst the data collectors (cold, wet, muddy, impatient) I envision a device about the size of a rugby football, padded, waterproof, with few controls: start, "still" (pause every ten minutes or so with the device placed on the floor (as during a crawl), or when you quit for the "night") and "stop", recording temperature, atmospheric pressure, and magnetic field. Precision and cost are of course opposites. Anyway, granted that you have the accelerometers and gyroscopes for two lots of three measurements, and a rapid scan of these parameters (time must be accurate!), in my ignorance I don't see what the problems are with interpreting the results. I would think that you'd calibrate the device for motion first by starting on a table (properly flat and level) and smoothly moving the device one metre (or yard, etc) in a straight line - say the x-direction and back, and again the y-direction. The z-direction is affected by gravity and providing the sensors are correctly aligned, you can thereby calibrate that. Then rotate the device carefully (interchanging xyz) and repeat. The resulting data record would be a sequence of integers in each direction, say the x-direction. You know that there was an acceleration in the x-direction, and then a negative acceleration. It started from v=0, attained some maximum v and then returned v to zero. You thus should be able to detemine the zero acceleration integer value (as when the device it at rest) and from the integral and known durations, determine the scale factor to produce an x displacement in the chosen unit (of metres, yards, feet) Depending on accuracy and the table (or floor) size, greater distances should be tried. Theorising further, there should be no need to align the movements with the physical sensor orientation, indeed I'd be tempted to consider having say three sets of sensors, each with their own orientation (and not orthogonal multiples) with a view to averaging the measurement, and for this purpose, three sets of sensors each of exactly the same orientation should each give the same numbers which is not what is wanted. Because a precision of one part in a thousand (0 to 1023) which is really +-500 is alas not so high, and as time wanders on, error grows (as sqrt(time) at a guess if you're in luck) Thus, whatever the orientation of the sensors in the device, your calibration would be with respect to the flat back of the device as defining the xy plane. Thus, with an arbitrary orientation of sensor, the initial state of motionless, flat on the table, will show where down is in the sensor orientation, and the movements in x and y and z will allow you to find the conversion to the device xy (its flat back) and z (perp. to its back) For field use, your device must be able to determine the direction of "down" so as to remove the standard acceleration of gravity. Thus the gyroscope readings, to determine change in attitude of the device (and thus the direction of down) so as to remove the unwanted acceleration so that the acceleration that remains applies to your change in position. The gyroscope orientation however does not rotate with the earth, so there are further details but I've typed enough already...

To take a stab at actually answering your question about the math, its not super hard. Start in 2 dimensions. Imagine a coordinate system in which your last known position is the origin (0,0). Every time you sample your guidance thingy, calculate your new position based on LAT/LON. Ignore the earth curve, the errors introduced by your guidance things will be many orders of magnitude greater than errors introduced by the errors caused by ignoring the earths shape.

I looked at doing this some months ago and essentially learned that it can't be done in any useful way over the long term (where "long" is more than a few seconds), but if you can make it work you'll be very wealthy. What might be handy is if you need really high speed positioning data, you could use it to figure out where you are between GPS readings, but that has issues too and its not what you are looking for.

NASA could not make it work when they were sending Apollo to the moon. They gave up and just had guys on the ground do the math and send the corrections tot he crew. Okay, that was a while ago, there is better technology now, but still not good enough. Then again, I believe it was Macdonald Douglas that was able to build an inertial guidance system to fly a plane from one city to another. It pretty much worked.

After some looking around, I found a report discussing inertial navigators which showed that the main error comes from the orientation, because it corrupts the correction for the direction of gravity, usually by far the largest acceleration being measured. An error in tilt of just 0·05 degrees leads to a position error of 8 metres in thirty seconds. This is rather disappointing. The precision of the angular velocity measurements must be high. All depends on the improvement of quality of the sensors, and solid-state (silicon) anglular velocity sensors are well under the quality of optical gyroscopes, which cost rather a lot more. Though, quality is improving all the time... The inertial guidance packages for an ICBM spare no expense in quality, and of course need operate only for about thirty minutes, max. Aircraft devices can get help from associated measurements such as locator beacons, barometric pressure, etc. though the software for such integration is complex. For cavers, there is still a possibility. If the device is moved then rested, moved then rested, and the software can recognise the periods of no motion, it can then correct for drift in its calculated state. An example suggested was a device in a walker's shoe: during the time the shoe is on the ground its velocity is zero, though I wonder about foot and shoe flexure as well as floor impact. Some caution is needed as if the shoe is on an escalator or in a lift, or other vehicle, the velocity is not zero. For cavers, I could imagine holding the device steady against the passage wall/atop a stalagmite at frequent intervals, and of course, in crawl sequences, there should be no problem with pauses...

In summary, it is possible to build a system using MEMS sensors that gives 5m of accuracy for only 1 minute.

That seems to be the current state. Sensor improvement is to be hoped for, and temperature dependency appears to be a significant factor. Returning to the original poster's question, if he is still puzzled by the mathematics, a post of the data received when performing a set of known and carefully described movements on a levelled surface (still, 1 metre (yard/etc) east, still for a second, back to the start, still, one unit north, still, etc.; rotate device ninety degrees, repeat etc.) then I think I could redact them into motion tracking parameters...

I figured out the math using good old fashioned Projectile motion back from college physics. I'm having some success with calculating the distance over time. What I've found out is the 8 MHZ sampling that I'm doing on the arudino pro 3.3 isn't enough I've stepped up to a 32-bit ARM Cortex M3 microprocessor running at 3.3V and 72Mhz. The propagation of error sampling at the Arduino rate of speed is really the problem. we have also come to the conclusion that after 1 hour 100ft of error is very feasible based on our initial test results.

I've been in correspondence with a team of engineers in Europe working on a PHD in IMU's that have had great success doing exactly what I'm doing. They are using slightly better 9 degree of freedom then I am using but my results should be sufficient for my needs. I am trying to extract knowledge from them on how they are doing this so nicely.

As to comment on the whole NASA thing actually the Space shuttle did use and INU/IMU unit. I work with an ex-nasa engineer and he confirmed that. also apollo crafts and moon landers had mechanical IMU's. I also work with an ex underwater ROV engineer and he has confirmed they use INU/IMU's as well for autonomous underwater vehicles so I believe that I have a team of engineers put together to solve the problem I've set out to solve.

being an engineer for over 10 years has taught me that anyone saying it can't be done just isn't trying.

Yes Apollo used mechanical IMUs, but most of the navigational smarts remained on Earth for weight and computational power considerations, and the on-board systems were regularly updated with manual star fixes.