Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

IMUFactor example noise/bias units #213

Closed
Jason420Git opened this issue Jan 17, 2020 · 10 comments · Fixed by #624
Closed

IMUFactor example noise/bias units #213

Jason420Git opened this issue Jan 17, 2020 · 10 comments · Fixed by #624
Assignees
Labels
docs Update to docs or README without code changes

Comments

@Jason420Git
Copy link

Hi, I am trying to use GTSAM to create a system using IMU and GPS measurements together in a factor graph.

I was following the example IMUFactor. It has the following values which were being computed from the sensor specifications


// We use the sensor specs to build the noise model for the IMU factor.
  double accel_noise_sigma = 0.0003924;
  double gyro_noise_sigma = 0.000205689024915;
  double accel_bias_rw_sigma = 0.004905;
  double gyro_bias_rw_sigma = 0.000001454441043;

However there are no units attached to these. I am trying to better model the IMU I am using which is Bosch BMI160. I cannot find any values which resemble these.

https://ae-bst.resource.bosch.com/media/_tech/media/datasheets/BST-BMI160-DS000.pdf

Any help in adapting the sample to use this IMU would be of great help!

@jingyibo123
Copy link

Did you solve the myth @Jason420Git

@mikesheffler
Copy link
Contributor

For what it's worth, here's a relevant disucssion from the mailing list last fall. A quick summary could be, "Read the IMU noise model wiki page over at the ETHZ Kalibr project", and, "converting between different units is hard."

If you take a look at that datasheet from Bosch, two numbers that jump out at me that will be relevant for you are, 'Output noise' (top of page 9) ug/sqrt(Hz) and 'Output noise' (near top of page 10) deg/s/sqrt(Hz) those are noises. To figure out how the noise rate changes, you pretty much need an Allan variance plot, so you

a) get that from the manufacturer
b) do the tests yourself
c) guess

with low-quality IMUs, guessing is not insane. The manufacturer will usually include a 'bias stability' or 'bias instability' number, and once you understand how Allan variance plots work, you can use that bias instability as an upper bound for the gyro random walk and accel random walk values.

Good luck!

@varunagrawal
Copy link
Collaborator

So I have made some progress on this, but not enough to have a satisfactory answer for everyone. Unfortunately, I have a deadline coming up next week so I am going to have to push this back until then.

I am assigning this to myself though and you can rest assured I will figure this out. :)

@varunagrawal varunagrawal added the docs Update to docs or README without code changes label Jul 10, 2020
@mikesheffler
Copy link
Contributor

mikesheffler commented Jul 11, 2020

So, I thought that the submitter was trying to find appropriate numerical values; if they just want the units, I would be shocked if they do not correspond to the IMU noise model at the Kalibr wiki. that is:

  • accel_noise_sigmaaccelerometer_noise_density
  • gyro_noise_sigmagyroscope_noise_density
  • accel_bias_rw_sigmaaccelerometer_random_walk
  • gyro_bias_rw_sigmagyroscope_random_walk

This is strongly implied by Forster et al., TRO '16 in footnote 2 on page 10, where the IMU parameters for the simulations are listed and match the units from the Kalibr wiki. Also, Kalibr is mentioned in the paper, although not explicitly in the context of calibrating IMU measurements.

As @dellaert said in the e-mail chain I linked above, Forster and Luca Carlone (cc @lucacarlone ) are probably the only ones that know for sure.

@lucacarlone
Copy link
Contributor

@mikesheffler : correct! the variables accel_noise_sigma, gyro_noise_sigma, accel_bias_rw_sigma, gyro_bias_rw_sigma are the continuous-time noise densities of the imu noise and the random walks (the discretization happens inside the factors). Therefore the units are the same as the ones mentioned in the Kalibr wiki, except a couple of (minor) typos in the Kalibr github: ethz-asl/kalibr#354
Let me know if you have further questions!

@varunagrawal
Copy link
Collaborator

varunagrawal commented Aug 11, 2020

So after spending a lot of time deep in the trenches with the code, and even understanding noise derivation via Allan Deviation plots, I can confirm what @lucacarlone already mentioned.

The units are:

  • gyro_noise_sigma: rad/s/√Hz (Gyro white noise or random walk)
  • accel_noise_sigma: m/s²/√Hz (Accelerometer white noise or random walk)
  • gyro_bias_rw_sigma: rad/s or rad √Hz/s (Gyro bias stability)
  • accel_bias_rw_sigma: m/s² or m √Hz/s² (Accelerometer bias stability)

@dellaert please recommend where should I put this info?

@zzodo
Copy link
Contributor

zzodo commented Jan 7, 2021

Love you guys! This helped me a lot 👍

@SubMishMar
Copy link

SubMishMar commented Mar 7, 2021

@varunagrawal
Can bias stability and random walk used interchangeably? For example if my IMU dataset says that the accelerometer bias stability (accel_bias_rw_sigma) is 0.0004 m/s^{2}, then, can I directly plug in the same value where the expected unit is m √Hz/s^{2} ? Or do I have to multiply the √ of my imu frequency to it.

I am a little confused about this.

@mikesheffler
Copy link
Contributor

Or do I have to multiply the √ of my imu frequency to it.

Yes, there is a conversion necessary. I don't believe the conversion involves the IMU frequency.

Take a look at this post on the Analog Devices forum . That person is talking about a gyroscope instead of an accelerometer, but their question is fundamentally similar to yours. Look at NevadaMark's answer. There, he's talking about values related to the (root) Allan variance curve, which (the units from the curve) are specified in deg / s. He's then converting those numbers to units to talk about values in deg / sqrt(hr) and deg / hr. Your problem doesn't involve units of hours, but note how he uses the sqrt in his conversion because of the units of the angle random walk vs the units of the bias stability.

@mintar
Copy link

mintar commented Nov 26, 2021

Can bias stability and random walk used interchangeably?

No.

Source: IEEE Standard 952-2020 for Specifying and Testing Single-Axis Interferometric Fiber Optic Gyros (page 18f)

Allan variance component unit
gyro angle quantization noise coefficient, Q ", μrad
gyro angle random walk (rate white noise) coefficient, N °/√h, rad/√s
gyro bias instability coefficient, B °/h, rad/s
gyro rate random walk coefficient, K (°/h)/√h, (rad/s)/√h
gyro ramp coefficient, R (°/h)/h, (rad/s)/s

Unfortunately the IEEE standard is only concerned with gyroscopes, not accelerometers. However, if we substitute rad/s with m/s², we get the following units for accelerometers:

Allan variance component unit
accelerometer velocity quantization noise coefficient, Q m/s
accelerometer velocity random walk (acceleration white noise) coefficient, N (m/s)/√s, (m/s)/√h
accelerometer bias instability coefficient, B m/s², m/h², mg (= milli-g-force, not milligram)
accelerometer acceleration random walk coefficient, K (m/s²)/√s, (m/s²)/√h
accelerometer ramp coefficient, R m/s³

These units are consistent with the following table from the Kalibr wiki (modified by me):

Kalibr Parameter Kalibr Wiki Symbol Allan variance symbol Units
Gyroscope "white noise" rad/s * (1/√Hz) = rad/(√s * √s) * √s = rad/√s
Accelerometer "white noise" m/s² * (1/√Hz) = m/(s * √s * √s) * √s = m/(s * √s) = m/(s * √s) * (√s/√s) = (m/s)/√s
Gyroscope "random walk" rad/s² * (1/√Hz) = rad/(s * √s * √s) * √s = rad/(s * √s) = rad/(s * √s) * (√s/√s) = (rad/s)/√s
Accelerometer "random walk" m/s³ * (1/√Hz) = m/(s² * √s * √s) * √s = m/(s² * √s) = m/(s² * √s) * (√s/√s) = (m/s²)/√s

Q and R are irrelevant for most practical applications, so mostly an IMU is only modeled using N, B and K or just N and K (but never N and B). I am pretty sure that gtsam only uses N and K.

Unfortunately, there's a lot of different terms for the same thing:

  • N: angle random walk, rate white noise, white noise, rate noise density, noise density
  • B: bias instability, in-run bias stability, flicker noise
  • K: rate random walk, random walk, bias

For Accelerometers, you have to substitute "angle" → "velocity" and "rate" → "acceleration"

Most IMU datasheets only list N and B. Also, sometimes they list rms (root mean squared error), which is not what we want; we want the standard deviation.

@mikesheffler wrote:

So, I thought that the submitter was trying to find appropriate numerical values; if they just want the units, I would be shocked if they do not correspond to the IMU noise model at the Kalibr wiki. that is:

* `accel_noise_sigma` → `accelerometer_noise_density`
* `gyro_noise_sigma` → `gyroscope_noise_density`
* `accel_bias_rw_sigma` → `accelerometer_random_walk`
* `gyro_bias_rw_sigma` → `gyroscope_random_walk`

Yes, this is correct.

@varunagrawal wrote:

The units are:

* `gyro_noise_sigma`: `rad/s/√Hz` (Gyro white noise)
* `accel_noise_sigma`: `m/s²/√Hz` (Accelerometer white noise)
* `gyro_bias_rw_sigma`: `rad/s` or `rad √Hz/s` (Gyro bias stability or random walk)
* `accel_bias_rw_sigma`: `m/s²` or `m √Hz/s²` (Accelerometer bias stability or random walk)

Not quite, at least from what I understand. I think this would be correct:

  • gyro_noise_sigma: rad/s/√Hz (Gyro white noise)
  • accel_noise_sigma: m/s²/√Hz (Accelerometer white noise)
  • gyro_bias_rw_sigma: rad/s or rad √Hz/s (Gyro bias stability or random walk)
  • accel_bias_rw_sigma: m/s² or m √Hz/s² (Accelerometer bias stability or random walk)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
docs Update to docs or README without code changes
Projects
None yet
Development

Successfully merging a pull request may close this issue.

8 participants