sleep and keeping awake the imu

[zaheeroz]

Hi Mateusz,
I come across another problem, doesnt seem a problem in your code but thought to ask, if yourself or anybody know how to resolve. The problem is, if I increase the sleep time or there is a delay of more than 300+ms ( I guess ) then angles/quaternions provide wrong results. So for example, with the delay, if I am at '0 degree pitch', move the system to 45 degree it will show something between 2-10 degrees. Commonsense ( at least mine ) says, if I take a read when the imu is at 45 degree it should say something near 45 even if I take read after 500ms.
My guess is, there is some sleep problem, the imu gets to sleep and after sleep its data is not accurate, which is understandable. So if I am correct, how not to let IMU sleep at all ? I googled and found, how to wake it but couldnt find how not to let it sleep at all. I am passing the ICM20948 data through my UKF too, which gives similar results so nothing is wrong with Madgwick. Also, above I am saying angles/quaternions, but they are calculated values so the real problem might be with accelerometer / gyro or magno.

Update1: I tried to write 0x00 and 0xBF to REG_ADD_PWR_MGMT_1 to keep the chip awake and it didnt work. The chip is very slow to respond to angle changes. It gets to the updated position but takes seconds to update. I am not sure, but when I said above that the angle goes upto 2-10 degress, that might have been due to slow response to change.

Thanks alot.
Zaheeroz

[mtmal]

Hi Zaheeroz,

It depends on the speed of the motion. Bear in mind, IMU provides rates of change (gyro - rad/s, acc - m/s/s). The only exception is a magnetometer, but it is only good for heading estimation (i.e. yaw) in an environment that does not impact the Earth's magnetic field. To obtain quaternions and angles, you need to integrate these measurements. AHRS or any Kalman Filtering are example tools for the job. So when you perform the integration, you want to have as many sample points as possible. Imagine you make a rapid step change from 0 to 45 deg. Gyro would increase from 0 to some value and then come back to 0 when the motion is stopped. If your sample rate is low, you may end up without any sample points on that curve, leaving you with two measurements - 0 rad/s before the motion and 0 rad/s after the motion. From this perspective, there is no change in the orientation whatsoever. There is a whole theory about sampling analogue signals (here: motion). The rule of thumb with IMU is to sample fast, usually at 100-500 Hz.

As for the IMU sleep, I don't think it can automatically go to sleep mode. According to the documentation, from what I read is that it enters sleep upon power-up. In the ICM20948::reset() we are already waking it up on line 368 and the only way to put the device to sleep would be to provide explicitly command it to do so. I believe that what you think about a possible sleep problem, is actually the infrequent sample rate discussed above.

In your update, you mentioned that the algorithm eventually reaches the actual orientation, but it takes a long time to get there. If you are using Madgwick, you need to amend the ICM20948::Config::mFramerate prior to initialisation. By default, it is set to 100Hz, but it needs to correctly reflect your sleep time.

Finally, apologies if I state the obvious, but remember that both gyro and acc have non-zero biases that drift (random walk). Madgwick takes that into account, but if you are implementing your own filter, you need to estimate them. In that case, having an additional sensor to correct IMU is handy. It is popular to use Inverse Kalman Filters that estimates errors rather than states. What you want to do, is to have an IMU on fast update rate, and other sensors to provide low-rate corrections for IMU errors (e.g. Visual Odometry, SLAM, or even Wheel Odometry in case of wheeled platforms.). IMU is problematic when stationary because measurement errors can be strengthened during the integration, so having a separate sensor to tell you when you are stationary is very helpful (at this point you either don't estimate using IMU or you focus on estimating biases, but it can be also hard as measurement errors could corrupt the estimation).

I hope I clarified few things for you. In a nutshell, you want to sample IMU frequently but it is good to understand why you have to do it and how to integrate IMU in your application to minimise its errors.

Let me know if you still experience issues. I'm happy to help.
All the best,
Mateusz

[zaheeroz]

Thanks Mateusz for your quick and detailed reply.

1. I understand what you said, and also I tested them, so there is no sleep problem.
2. Gyro is 'rate of angle change', agreed. However, when I run your ICM20948_app, without any (additional) delay and put the device at x-degree it keeps giving me that degree ( which is a correct response) however, it doesn't comply with the statement that Gyro gives 0 degree (rate) after some time so the whole thing becomes 0 (because of integration ). It doesn't do that with your Madgwick implementation ( which again I think is as expected and correct ), however it goes to 0 with my Unscented Kalman Filter (UKF). So I was thinking something's wrong with my UKF.
3. I am using UKF to predict the state as well as the errors, my system is a UAV and I am teaching myself everything related so kinda alot for me to take-in.
   So my question is : Why the Madgwick is not getting back to 0 when the system is lying down with an angle.
   Thanks again really helpful.
   Zaheeroz

[mtmal]

It's really great that you are teaching yourself and I appreciate there is a lot in the world of UAV or robotics in general.

Regarding your question: "Why the Madgwick is not getting back to 0..." do you mean angle or rate?

If you place IMU at X-degree, the gyro rate should drop back to zero and because of the integration, your angle should be X (not zero, apologies if I confused you). Even then, your gyro may not show exactly zero, because of the noise. The mean value would be your bias which you need to estimate as well (Madgwick should take that into account but you need to include it in your UKF implementation). However, when using the Kalman filter you may get problems with observability because from one measurement you are trying to estimate two values: angle and bias.

I have a feeling that I did not fully understand your question, though...
Mateusz

[zaheeroz]

I meant the angle of the Madgwick doesnt get back to 0 despite the rate is 0 at some point. and thats the behavior of ICM20948_app atm, which is the required behavior. I thought you were saying that as its integrating so at some point both rate and angle should get to 0. But its clear to me now that only rate gets to 0 not the angle.
However, I am unable so far to get the same behaviour from UKF, despite that UKF is very smooth, I am simulating the quaternion to see the change of angles and overall behaviour of the system.

[mtmal]

Ok, I think we are getting there. The fact that the rate goes back to zero does not imply that the angle would go to zero as well. Gyro will give you a non-zero value when there is a motion. So when you move IMU from zero to X-degree, your gyro will bump up and then go back to zero when the motion stops at X-degree. If you integrate the area under the curve of a gyro (rad/s) against time (s) you should get your desired X (rad). What I mean in my previous post, was that if your sampling rate is low and thus you measure only gyro before the motion, and after the move, it would give you 0 rad/s for both measurements. Integrating these two would give you zero degree orientation. It is like driving a car: you start with zero velocity and when you stop your velocity is zero as well. But if you integrate it during your drive, you will get the total distance travelled.

I don't know what your mathematical model in UKF looks like, but if you are only integrating gyro, it may not be enough due to bias. The simplest assumption for how angle would look like is
angle[k+1] = angle[k] + dt * (angle_rate[k] + bias[k])
where
angle_rate[k] = measured_rate[k] + measurement_noise[k]
is your measurement with known covariance, and
bias[k+1] = bias[k] + process_noise[k]
is your bias driven by a process noise. You would need to arbitrarily select the covariance of this noise - it is always hard with process noise.
The problem is that you only measure angle_rate, where the bias is unobservable. What Madgwick does, it takes also an accelerometer and magnetometer (if available) into account to estimate this bias. You would need to replicate similar behaviour in your model or introduce another sensor that would give you orientation information, making the gyro bias observable.

[zaheeroz]

You made many things very simple for me. I am sure I will fix them now :)
thanks alot and have a great day.
Zaheeroz