I currently have a platform that has a 6DOF IMU (the LSM6DS3) with no magnetometer. I want to get as accurate of a relative yaw angle reading as possible, avoiding or minimizing any gyro drift. I tried a simple approach of:
a) calculate the gyro angular rate zero-offset by computing the average gyro angular rate reading for a few seconds while my platform is known to be stationary.
b) read the angular rate while the LSM6DS3 reports a new reading is available (so, this should essentially be the output data rate I configured, i.e. 416Hz). I subtract this from the zero-offset calculated in (a) above to get the degrees/s angular rate and then integrate (multiply the time delta with the degrees/s angular rate and add to the current yaw angle).
Is this the best I can do to avoid gyro drift issues if I want to get as accurate of a relative yaw rating possible?
I looked a little bit at Kalman filters and Madgwick filters and Mahony filters but they don't seem to suggest the ability to improve yaw angle readings as it seems the accelerometer would not be useful to calculate the yaw angle? Is that correct?
Related
I am currently working on a mission to fuse GNSS and IMU for a more accurate navigation system for autonomous vehicles. I am very familiar with using GNSS to get the accurate position, however I'm a newbie in using IMU sensor. I've read several kinds of literature but am still confused about which better way should I do to remove bias from the accelerometer and gyroscope measurement.
I have 2 kinds of raw measurement data using MPU-9250, they are acceleration data (m/s2) in the x,y, z-axis and angular velocity data (deg/s) also in the x,y, z-axis. I have tried to input these data into my sensor fusion program. Unfortunately, I got unsatisfied with accuracy.. Hence I think firstly I should correcting (removing bias) of raw data IMU, and then the corrected IMU data can be input to my fusion program.
I couldn't find an answer that my brain could understand or fit my situation. Can someone please share some information about this? Can I use a high-pass filter or a low-pass filter in this situation?
I would really appreciate if there is someone could explain in detail to me without using complex math formulas/symbols, I'm not a mathematician and this is one of my problems when looking for information.
Thank you in advance
Accelerometer and Gyroscope have substantial bias usually. You could break the bias down to factors like,
Constant bias
Bias induced by temperature variation.
Bias instability
The static part of bias is easy to subtract out. If the unit starts from level orientation and without any movement, you could take samples for ~1s, average it and subtract it from your readings. Although, this step removes a big chuck of bias, it cannot still fully remove it (due to level not being perfect).
In case you observe that the temperature of IMU die varies during operation (even 5-10 deg matters), note down the bias and temperature (MPU9250 has an inbuilt temperature sensor). Fit a linear or quadratic curve that captures bias against temperature. Later on, use the temperature reading to estimate bias and subtract it out.
Even after implementing 1 and 2, there will still be some stubborn bias left. If the same is used in a fusion algorithm like Kalman filter (that is not formulated to estimate bias, the resulting position and orientation estimates will be biased too).
Bias can be estimated along with important states (like position) using some external reference/sensor like GNSS, Camera.
Complementary filter (low pass + high pass) or a Kalman filter can be formulated for this purpose.
Kalman filter approach:
Good amount of intuition along with some mathematics is needed to use this approach. Basically the work involves formulating prediction & measurement model and then provide rough noise variances for your measurements and prediction. An important thing to understand is that, Kalman filter assumes that the errors follow normal distribution without any bias. So the formulation should deliberately put bias terms as unknown states that should be estimated too (Do not assume that the sensor is bias free in the formulation)..
You could checkout my other answer to gain a detailed understanding of this approach.
Complementary filter approach
Complementary filter is simpler for simpler problems :P
The idea is that we use low pass filter on noisy measurement and high pass filter on biased measurement. Then add them up and call it a day.
Make sure that both the LPF and HPF are complements of each other (Transfer function of HPF should be 1-LPF). Typically first order filters with same time constants are used. Additionally the filter equations have to be converted from continuous laplace domain to discrete form (Read about ZOH, Tustins approximation...).
The final form is scattered around the internet too.
Personally I would use a Kalman filter for this purpose, but complementary filter can be used with same amount of effort. You could do this,
Assume that the body is not accelerating on average in long term (1-10 s or so). Then you could say that the accelerometer measures the direction of gravity in long term relative to the IMU. Then arctan(accy, accz) can be used to obtain an estimate of pitch and roll. But this pitch and roll readings will suffer from substantial noise. Implement a low pass filter on it with time constant ~5 seconds or so. Additionally add the latest pitch/roll with dt*transformationMatrix*gyroscope to get another pitch and roll. But these suffer from bias. Implement a HPF over gyro based Pitch and Roll. Add them together to get Pitch and Roll. Lets call these IMU_PR.
Now forget our original acceleration assumption. accelerometer gives specific force (which is net acceleration - gravity). Since we have Pitch and Roll angles (IMU_PR), we know gravities direction. Add gravity to accel readings to get an estimate of acceleration. Apply proper frame conversion to bring this acceleration to same coordinate frame as GPS (you will need an estimate of Yaw to do so. Fuse a magnetometer with gyroscope for this purpose). Then do vel = vel + acc*dt. Integrate it again to get an estimate of position from IMU. But this will drift due to the bias in accelerometer (and pitch, roll). Implement a high pass filter over this position and low pass filter over GPS position to get a final estimate.
Currently, we are using BNO055 in one of our projects. The IMU is placed next to the dc motor due to space constraints within the hardware setup. Due to motors vibrations, we are applying a low pass filter on quaternion values read from this (https://github.com/adafruit/Adafruit_BN ... awdata.ino) script. We have set 5 Hz as a cut-off frequency of the filter. We have also placed IMU on Sorbothane (damping material) to minimize the vibrations. However, we are still selling the error in the orientation.
What could be done to reduce the impact of motor vibrations on IMU both from a software and hardware point of view? Any inputs are highly appreciated.
Motor vibration may not be the only problem here.
Orientation estimation can go wrong due to multiple factors like,
Bias due to incorrect calibration. Keep the sensor level and idle. Make sure Gyroscope reads close to (0,0,0) on average. Accelerometer should read either (0,0,9.81) or (0 0 -9.81) m/s^2 depending on the convention.
Bias due to temperature changes. Even a 10 deg change in PCB temperature can change the bias in Gyro by 0.3 dps (according to the datasheet)
Motor noise. Seems like you have already tried reducing this one.
If none of them work, you could try implementing your own Kalman or complementary filter based on the raw data from Gyro, Accel and mag. This way you can be sure about calibration process, estimator gains, how the estimator works.
If implementing Kalman filter is difficult, you could try this AHRS filter block/algorithm given here,
https://in.mathworks.com/help/nav/ref/ahrsfilter-system-object.html
I have acceleration data from few accelerometer sensors. I am interested in the displacement so, I double integrate it (cumtrapz in matlab). The acceleration data is really noisy so, I use Butterworth filter to filter out the low frequency noises. After the first integration, I get the velocity and then filter this velocity to further integrate to get displacement. Everything works just fine. The only issue I have is that after filtering I lose the actual value of the acceleration and thus the value of the displacement I get does not make any sense.
Any suggestions?
Over the past year I have used many different methods of combining Accelerometers, gryos and Magnetometers to get accurate readings of Head angles.
I have also started looking into using a Kalman filter to further improve these readings.
Yet I am still to find a method of removing external magnetic field influences using the other sensors, for example;
If my heading angle was accurate, and suddenly an external magnetic field approaches, my heading angle will be influenced, but to my gyro and accelerometer I haven’t moved.
Is there any algorithms or calculations anyone can think of to override the magnetometer in a way that can determine whether you have moved or not?
Any help would be much appreciated!
One simple solution is to use the gyro/accelerometer as you mentioned, and combine that with delayed filtering, where you wait for a couple of seconds before providing an estimate of the attitude.
Compute the short term attitude from gyro/accel only (start with any arbitrary heading) using gyro integration with accel measurements, and then compute the short term attitude from the magnetometer/accel only using, say TRIAD. Compute the error between these two quantities and decide on a threshold. If the you exceed the threshold, it means there is a magnetic disturbance, and you can thus stop using it in your attitude solution. If they are within threshold, you can continue using the magnetometer.
If you think of more metrics to decide whether you are in a magnetic disturbance or not (such as the magnetometer norm rising to a ridiculous number), then you can add those metric to an HMM, which will combine these metrics and give you an estimate of whether you are in a disturbance or not.
Using just the rotationRate property of Core Motion or Gyroscope is it possible to extract how much radians (or degrees if you like) the device rotated?
I have tried to do a timed sampled of Core Motion data, for example, sampling it 5 times per second so I know that there's 0.2seconds between each reading. Then if I have a rotationRate of 0.5 radians per second from one read to another, in theory I could divide this by 5 and know now much radians the device rotated since the last time.
This seems logical, but the results have nothing to do with reality. Rotating the device 90 degrees will produce results telling me that the device rotated 100 times less than that.
Is it possible to extract how much the device rotated just by looking at rotationRate?
What am I missing?
Need more space than a comment ;-) iOS retrieves all data in radians and if your other calculations are correct, I thought it might be the angle measured in radians.
In general your approach seems to be alright: Take every signal's angle velocity, multiply it with time delta and you will have the angle delta for this timeframe. Then sum all your angles and the result should be the covered distance as angle in radians. Angle phi is the integral of angle velocity omega over the elapsed time and doing numerical integration with the trapezoidal rule (i.e. like described) is OK for gyroscope's data (not for accelerometer).
In general I would recommend to use the timestamp delivered by core motion instead of the defined period (1/5) as recommended by Apple, because device motion data is often delivered in a lower frequency than expected (see What is the official iPhone 4 maximum gyroscope data update frequency and or Push method for core motion and frequency of Accelerometer/Gyroscope Data.
Furthermore you should take a higher frequency to avoid errors in your numerical integration.
[Update from comments section:]
If your are interested in integrating via extended Simpson's Rule I recommend this paper (German only, p. 173 ff.) and An Extension to Newton-Cotes Formulae. Some sample code as extracted snippet taken from an existing project can be found here: DevicePosition.m Note that it might not compile, no warranty, as is, ... you know this from other sites ;-)
A free app displaying sensor input as graphs for iPhone: Sensor Monitor