CODEWITHSUNDEEP
c++caccelerometernumerical-integrationinertial-navigation
ImQ009
ImQ009

Reputation: 335

Double integrating acceleration in C++ using a 9DOF IMU with sensor fusion

I've spent a few hours doing research on numeric integration and velocity/position estimation but I couldn't really find an answer that would be either understandable by my brain or appropriate for my situation.

I have an IMU (Inertial Measurement Unit) that has a gyro, an accelerometer and a magnetometer. All those sensors are in fusion, which means for example using the gyro I'm able to compensate for the gravity in the accelerometer readings and the magnetometer compensates the drift. In other words, I can get pure acceleration readings using such setup.

Now, I'm trying to accurately estimate the position based on acceleration, which as you may know requires double integration, and there are various methods of doing that. But I don't know which would be the most appropriate here. Could somebody please share some information about this? Also, I'd appreciate it if you could explain it to me without using any complex math formulas/symbols, I'm not a mathematician and this was one of my problems when looking for information.

Thanks

Upvotes: 4

Views: 9376

Answers (1)

Gouda
Gouda

Reputation: 1005

You can integrate the accelerations by simply summing the acceleration vectors multiplied by the timestep (period of the IMU) to get the velocity, then sum the velocities times the timestep to get the position. You can propagate (not integrate) the orientation by using various methods depending on which orientation representation you choose (Euler angles, Quaternions, Attitude Matrix (DCM), Axis-Angle, etc..).

However you have a bigger problem.

Long story short: Unless you have a naval-quality IMU (USD$200,000+) you cannot simply integrate the accelerations and angular rates to get an accurate pose (position and orientation) estimates.

I assume you are using a low-cost (under USD$1,000) IMU - your accelerometer and gyro are subject to both noise and bias. These will make it impossible to get accurate pose by simply integrating.

In practice to do what you are intending it is required to fuse 'correcting' measurements of the position and optionally the orientation. The IMU 'predicts' the position/orientation, while another sensor model (camera features, gps, altimeter, range/bearing measurements) take the predicted position and 'correct' it. There are various methods of fusing this data, the most prolific of which is the Extended Kalman Filter or Error-State (Indirect) Kalman Filter.

Back to your original question; I would represent the orientation as quaternions, and you can propagate the quaternion orientation by using the error-quaternion derivative and the angular rates from your gyro.

EDIT:

The noise problem can be partially worked around by using a high pass filter, but what kind of bias are you exactly talking about?

You should read up on the sources of error in MEMS accelerometers: constant alignment bias, random walk bias, white noise and temperature bias. As you said you can high-pass filter to reduce the effect of noise - however this is not perfect so there is significant residual noise. The double integration of the residual noise gives a quadratically-increasing position error. Even after removing the acceleration due to gravity there will be significant accelerations measured due to these error sources which will render the position estimate inaccurate within less than 1 second of integration.

Upvotes: 7

Related Questions