IMU Noise Modeling

Inertial measurement units (IMUs) are used in many SLAM procedures. In most cases, noise parameters must be specified for the IMU used. But what exactly do these parameters actually refer to?

IMUs with six degrees of freedom (6-DoF) consist of an accelerometer and a gyroscope. These measure the acceleration (\(m/sec^2\)) and the angular velocity (\(rad/sec\)) along three Cartesian axes in the sensor frame. The earliest IMUs were mechanical gyroscopes and accelerometers, used primarily in aviation and marine navigation. In the 1950s and 1960s, the first strap-down IMUs for guided missile technology were introduced, with bulky and expensive components that relied on precision machining and analog electronics. In the 1970s and 1980s, the first solid-state gyroscopes and accelerometers using piezoelectric and vibrating structure technologies were developed. These were smaller, more reliable, and less expensive than their mechanical predecessors. The emergence of Micro-Electro-Mechanical Systems (MEMS) revolutionized IMUs in the 1990s, enabling the miniaturization of sensors. MEMS IMUs became significantly cheaper, smaller, and more power-efficient, allowing for widespread commercial use. Today, MEMS-based IMUs are standard and are constantly being developed further.

IMU measurement model

If sensors were error-free, this would make many things in robotics (and beyond) easier. But as is also the case with other sensors, IMU measurements are subject to errors. A distinction can be made between systematic errors and random noise. The bias is a systematic error that leads to a constant or slowly changing offset of the measured values. White noise, on the other hand, has no systematic tendency and is mainly caused by thermal noise and other random electronic fluctuations within the sensor circuit. White noise leads to a spread of the measured values around the true value, resulting in unsharp or noisy signal output. Taking the bias and white noise into account, the accelerometer measurement at time \(t\) can be modeled as follows:

\[\tilde{a}(t)_a = a(t)_a - R_{aw}(t) \cdot g_w + b_a^a(t) + \eta_a^a(t) \]

Here, \(\tilde{a}(t)_a\) is the measured acceleration in the accelerometer frame, and \(a(t)_a\) is the true acceleration within the accelerometer frame without considering gravity. \(R_{aw}(t)\) is the rotation matrix that rotates the gravity vector \(g_w\) from the world frame to the accelerometer frame. \(b_a^a(t)\) and \(\eta_a^a(t)\) are the accelerometer bias and white noise in the accelerometer frame. The gyroscope measurements can be modeled similarly:

\[\tilde{\omega}(t)_g = \omega(t)_g + b_g^g(t) + \eta_g^g(t) \]

All quantities of the accelerometer can be found equally in the gyroscope, except for gravity. The question now arises as to how the quantities \(b_a^a(t)\), \(b_g^g(t)\) and \(\eta_a^a(t)\), \(\eta_g^g(t)\) are characterized.

Bias

Starting with the bias, it might seem confusing to model the bias as time-dependent, since it can be considered as a constant offset. Then the bias would be modeled by a constant \(b(t) = b_0\) (constant bias error). However, the bias also has a dynamic component: the random walk of the bias, described by the rate random walk (RRW). Then the bias becomes:

\[b(t + \Delta t) = b(t) + \eta_b(t), \quad \eta_b(t) \sim \mathcal{N}(\bm{0},\,\Sigma_b)\]

Here, \(\Sigma_b\) is a diagonal covariance matrix with \(\sigma_{b,x}^2, \sigma_{b,y}^2, \sigma_{b,z}^2\) on its diagonal. \(\sigma_{b,x}, \sigma_{b,y}, \sigma_{b,z}\) are the standard deviations of the bias random walk for all three Cartesian axes. The RRW describes the small random changes (or drift) of the bias and is often called brown noise term. Considering the evolution of the bias, the value to be subtracted from the measurement can change over time. If this development is neglected, an offset is integrated over time, and the integrated pose estimate drifts. The RRW is specified in the unit \(\frac{rad}{sec^2} \cdot \frac{1}{\sqrt{Hz}} = \frac{rad}{sec^{1.5}}\) for gyroscopes and in \(\frac{m}{sec^3} \cdot \frac{1}{\sqrt{Hz}} = \frac{m}{sec^{2.5}}\) for accelerometers. In tools like Kalibr the RRW affecting the bias is called gyroscope random walk and accelerometer random walk, respectively. In Kalibr and many SLAM applications, only one standard deviation is provided for the bias random walk (and other noise quantities). This is done to simplify the model further by assuming the noise to be similar along all axes. This can be achieved, for instance, by taking the mean value of all standard deviations.

White noise

In addition to the bias, the white noise (\(\eta_a^a(t)\), \(\eta_g^g(t)\)) is to be considered. This is not a systematic error but is assumed to be random noise. The sensor output signal is perturbed by a type of thermo-mechanical noise fluctuating at a higher rate than the sample rate of the sensor, called angle/velocity random walk (ARW/VRW). So, these are not (slowly changing) constants that need to be substracted from the measurements, but random quantities. They are modeled as Gaussian distributions with zero mean:

\[\eta_a^a(t) \sim \mathcal{N}(\bm{0},\,\Sigma_a^2), \quad \eta_g^g(t) \sim \mathcal{N}(\bm{0},\,\Sigma_g^2)\]

The ARW and VRW are specified in \(\frac{rad}{sec} \cdot \frac{1}{\sqrt{Hz}} = \frac{rad}{sec^{0.5}}\) for gyroscopes and in \(\frac{m}{sec^2} \cdot \frac{1}{\sqrt{Hz}} = \frac{m}{sec^{1.5}}\) for accelerometers. Derived from their naming conventions, these quantities cause random deviations in angle or velocity when they are integrated over time. In Kalibr the ARW and VRW are called gyroscope noise density and accelerometer noise density.

Power Spectral Density of noise

So, there is white noise and brown noise affecting IMU measurements. In addition to these two noise parameters, there is also pink noise: the so-called bias instability (or in-run stability). This color of noise is typically not considered in many SLAM methods because it is harder to predict and, in SLAM scenarios, usually has an effect that is small enough to be neglected. Nevertheless, for the sake of completeness, it is also of interest to have a short look at it.

The bias instability is given in \(\frac{rad}{sec}\) and \(\frac{m}{sec^2}\). It manifests as a slowly varying error in the sensor's output, which means that the sensor's reading might drift gradually away from the true value over time. This drift is not random in the short term but appears to be a wandering or random walk of the bias over longer periods. Modeling pink noise often requires more complex algorithms that are more difficult to implement and calculate for real-time applications. SLAM methods need to work in real time, so usually a simpler model is chosen.

All these colors of noise can be characterized by analyzing the power spectral density (PSD) of (IMU) noise. If \(\eta\) is the overall IMU noise, then \(S_\eta(f)\) describes the PSD of noise as a function of the frequency \(f\):

\[S_\eta(f) = N^2 + \frac{B^2}{2 \pi f} + \frac{K^2}{(2 \pi f)^2} \]

It can be assumed that these frequency components are statistically independent. Therefore, the noise signal \(\eta(t)\) can be modeled to consist of white (\(N\)), brown (\(B\)) and pink (\(K\)) noise:

\[ \eta(t) = \eta_N(t) + \eta_B(t) + \eta_K(t) \]

To determine all IMU noise parameters commonly used in SLAM methods (i.e., ARW, VRW and RRW) the Allan variance can be used. In order not to exceed the scope of this article, the Allan variance will be discussed in detail in a future article.

Further reading and sources

  1. https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model
  2. https://stanford.edu/class/ee267/lectures/lecture9.pdf
  3. https://www.tangramvision.com/blog/stochastic-imu-error-modeling
  4. https://github.com/ori-drs/allan_variance_ros
  5. Characterization of Errors and Noises in MEMS Inertial Sensors Using Allan Variance Method
  6. https://nitinjsanket.github.io/tutorials/attitudeest/kf
  7. https://de.mathworks.com/help/nav/ug/inertial-sensor-noise-analysis-using-allan-variance.html