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?
Related
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?
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.
do you know maybe where I can find code or example for velocity estimation from IMU (acc+gyro+magnetometer) data. I calculated biases from data where IMU stands still. I want to implement velocity estimation with some kind of filter (Kalman/Complementary) but I can't find any. I also have camera velocity estimation, maybe it can help as some kind of fusion?
Thank you in advance!
Kind regards
I don't have an example code that exactly works for your case. But this approach can help (based on past experience),
Kalman filter:
Decide and formulate the states X, control inputs U, outputs, prediction and observation equations.
Implement/ reuse some implementation of Kalman Filter. Here is a Simulink based implementation for reference.
Set the measurement noise and prediction error variances. It may require some fine tuning later.
Verify that the KF works against some reference. If you have another way to measure velocity, check the KF velocity against it.
States and Control inputs:
States could be a array containing
Linear velocities [Vx, Vy, Vz]
Angular velocities [omega_x, omega_y, omega_z]
Bias in gyroscope. This bias is largely constant but can change with temperature and other factors. Accelerometer measurements will be used by KF to correct for gyro bias.
Bias in Accelerometer. This bias is largely constant but can change with temperature and other factors. Camera velocity will be used by KF to correct for accel bias.
Orientation (Euler angles or quaternion)
Control inputs need not be the actual commands that are being sent to your actuators.
In this case, control inputs can be the net force or net acceleration which is,
Accelerometer data (Which is specific force) + Acceleration due to gravity
Prediction equations:
Prediction equations predict the states for next time step based on current states and control inputs.
This MathWorks documentation has a good reference for prediction equations relevant to IMU.
Observation/measurement model:
Relates measurements with states.
Accel data is already used in prediction. Ignore it here.
Gyro data is [gx, gy, gz] = [omega_x + gyro_bias_x, ....] + errors
One way to handle magnetometer is to obtain yaw angle from it - arctan(y/x) and then use the yaw_mag as measurement.
Camera data is [vx_cam, vy_cam, vz_cam] = [Vx, Vy, Vx] + errors
Finally append all the rows and bring it to Y=C*X + noise form.
Y denotes the measurements from different sensors and X represents the states.
Y would be [gx, gy, gz, yaw_mag, vx,cam, vy_cam, vz_cam] in this case.
Disclaimer: I am a MathWorks employee and links are shared from MathWorks documentation.
I have a doubt working with Simulink to carry out the integration of an acceleration to obtain the speed and then the displacement.
The thing is that I actually know the value that the displacement has to finally give, and it's not the same thing that I get by integrating.
However, if I derive the displacement calculated and the correct displacement I obtain two velocities that are equal and coincide with the velocity calculated from the integration of the acceleration. In the same way if I derive this two velocities I obtain the same accelrations that are the same with the original one.
I think the mistake must be in the constant term that is generated integrating but I am quite confused with the results coinciding so well when I derive from an erroneous value.
Does anyone know how I could solve this error and effectively get the displacement? Is it possible knowing only the acceleration?
I attach images of both the assembly and the results obtained.
Greetings and thanks in advance
pic1. Assembly
pic2. Acceleration: Original, Obtained from the speed from the wrong displacement, Obtained from the speed from the correct displacement
pic3. Velocities: Integrated from original acceleration, Obtained from the wrong displacement, Obtained from the speed from the correct displacement.
pic4. Displacement: Corrrect displacement, Obtained displacement.
I have accelerometer readings of three axis X, Y and z, will be getting data in a frequency of (62 records per second). Could you please suggest me how can I calculate the displacement.
Data in hand:
Accelerometer readings with respect to time.
Do I need to calculate the displacement using time domain data or need to convert into frequency domain. Which one will give a accurate results?
You can double integrate the acceleration vector over time to obtain the displacement. In theory this is a perfectly sensible solution.
But in practice, there will always be a component of g (acceleration due to gravity) acting on at least one of the axes all the time. Let's say you subtract the g component from your xyz vectors. The problem is that any slight error in readings (even off by a small order of magnitude) when double integration will lead to the error adding up over time rendering the displacement wildly inaccurate.
According to the integrated values, you will most likely see even an idle object fly off into space. You'll need an additional sensor to tell you the orientation - like a gyroscope, and have some point of reference (the Wiimote does this with an IR sensor).
This is primarily a time domain problem, but you could have a frequency domain stage where some amount of filtering is done to remove measurement error or process error.
tl;dr Positional tracking with acceleration sensors alone is a hard problem.