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.
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.
Model approach:
I am modelling on Matlab-Simulink a very thin flexible structure. All points of the model are link with each other with springs and dampers this way (without the tethers in the center):
Mesh description
The general equation of my model applied at each point of the mesh is the following:
Dynamic formula of mass/spring/damper system
With k the springs stiffness, and c the dampers damping.
To adapt the physical properties of the material I want to model, the spring stiffness has been set to a very high value, around k = 5000. This mean that my spring links are highly reactive to any deformation.
Problem:
This leads to my problem: High stiffness links induce high frequency displacement that I can consider as noise in the simulation.
The simulation is much slower as the variable time step, I am using must be very low.
This high-frequency displacements (around 160 Hz, which the resonance frequency of the springs) stays all along the simulations.
Here is a simulation of my structure rotating at a constant angular speed:
In-time evolution of a random point of my structure in spherical coordinates
We can see that R is vibrating at a very high frequency. However, the displacement amplitude is clearly negligible.
To speed up the simulation, I want to suppress those vibrations!
Investigation:
To suppress them, I investigate on signal filtering techniques, mainly low-pass filtering. On every loop of our simulation, and what should enter our filter are data of all my points in all my axis.
Simulink low-pass filter block
The continuous version of low-pass filter in Simulink library has been tested on the acceleration, the speed and the position, with several cut-off frequencies from 100 Hz to 500 Hz.
For example, for a cut-off frequency of 200Hz and filtering the position at t=0.6 sec I have:
In-time filtered evolution of a random point of my structure in spherical coordinates
It is an in-plane movement so I don’t have any elevation angle, but azimuth angle and point distance from the center are completely diverging.
The problem might come from:
The fact that I am in a closed-loop system
The fact that for the mesh we have, the filter receives 81 vectors of 3*1 at each time step and maybe the filter block is not made to function with that.
The fact that for the mesh we have, the filter receives 81 vectors of 3*1 at each time step and maybe the filter block is not made to function with that.
Main question:
Are there filtering techniques for closed-loop and multiple inputs system that could solve my problem?
Digital filter designer works with SISO signals. Just demux your signals and apply some lowpass filters. You gave lots of info that made it harder to understand the core problem, if there is anything else you can re-iterate. I'd start with a 3rd order Butterworth LPF wc at around 100Hz for your needs.
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 implemented an easy mathematical model of a pendulum on simulink. I implemented a Kalman filter to estimate the velocity state having as input a zero torque, the initial position set to a certain angle (pi/18) and the initial velocity set to zero.
I suppose to have just a position measurement as the output of the system and the goal is to have a good estimation of the states. I expect to obtain a good estimation of the angle and the velocity but, as far as the velocity estimation is concerned, it is totally wrong.
I think there is something wrong in the block diagram. The question is: it is possible to have a "good" estimation of both the state using a Kalman filter for a mechanical linear Pendulum?
I have been trying to implement a navigation system for a robot that uses an Inertial Measurement Unit (IMU) and camera observations of known landmarks in order to localise itself in its environment. I have chosen the indirect-feedback Kalman Filter (a.k.a. Error-State Kalman Filter, ESKF) to do this. I have also had some success with an Extended KF.
I have read many texts and the two I am using to implement the ESKF are "Quaternion kinematics for the error-state KF" and "A Kalman Filter-based Algorithm for IMU-Camera Calibration" (pay-walled paper, google-able).
I am using the first text because it better describes the structure of the ESKF, and the second because it includes details about the vision measurement model. In my question I will be using the terminology from the first text: 'nominal state', 'error state' and 'true state'; which refer to the IMU integrator, Kalman Filter, and the composition of the two (nominal minus errors).
The diagram below shows the structure of my ESKF implemented in Matlab/Simulink; in case you are not familiar with Simulink I will briefly explain the diagram. The green section is the Nominal State integrator, the blue section is the ESKF, and the red section is the sum of the nominal and error states. The 'RT' blocks are 'Rate Transitions' which can be ignored.
My first question: Is this structure correct?
My second question: How are the error-state equations for the measurement models derived?
In my case I have tried using the measurement model of the second text, but it did not work.
Kind Regards,
Your block diagram combines two indirect methods for bringing IMU data into a KF:
You have an external IMU integrator (in green, labelled "INS", sometimes called the mechanization, and described by you as the "nominal state", but I've also seen it called the "reference state"). This method freely integrates the IMU externally to the KF and is usually chosen so you can do this integration at a different (much higher) rate than the KF predict/update step (the indirect form). Historically I think this was popular because the KF is generally the computationally expensive part.
You have also fed your IMU into the KF block as u, which I am assuming is the "command" input to the KF. This is an alternative to the external integrator. In a direct KF you would treat your IMU data as measurements. In order to do that, the IMU would have to model (position, velocity, and) acceleration and (orientation and) angular velocity: Otherwise there is no possible H such that Hx can produce estimated IMU output terms). If you instead feed your IMU measurements in as a command, your predict step can simply act as an integrator, so you only have to model as far as velocity and orientation.
You should pick only one of those options. I think the second one is easier to understand, but it is closer to a direct Kalman filter, and requires you to predict/update for every IMU sample, rather than at the (I assume) slower camera framerate.
Regarding measurement equations for version (1), in any KF you can only predict things you can know from your state. The KF state in this case is a vector of error terms, and thus you can only predict things like "position error". As a result you need to pre-condition your measurements in z to be position errors. So make your measurement the difference between your "estimated true state" and your position from "noisy camera observations". This exact idea may be represented by the xHat input to the indirect KF. I don't know anything about the MATLAB/Simulink stuff going on there.
Regarding real-world considerations for the summing block (in red) I refer you to another answer about indirect Kalman filters.
Q1) Your SIMULINK model looks to be appropriate. Let me shed some light on quaternion mechanization based KF's which I've worked on for navigation applications.
Since Kalman Filter is an elegant mathematical technique which borrows from the science of stochastics and measurement, it can help you reduce the noise from the system without the need for elaborately modeling the noise.
All KF systems start with some preliminary understanding of the model that you want to make free of noise. The measurements are fed back to evolve the states better (the measurement equation Y = CX). In your case, the states that you are talking about are errors in quartenions which would be the 4 values, dq1, dq2, dq3, dq4.
KF working well in your application would accurately determine the attitude/orientation of the device by controlling the error around the quaternion. The quaternions are spatial orientation of any body, understood using a scalar and a vector, more specifically an angle and an axis.
The error equations that you are talking about are covariances which contribute to Kalman Gain. The covariances denote spread around the mean and they are useful in understanding how the central/ average behavior of the system is changing with time. Low covariances denote less deviation from the mean behavior for any system. As KF cycles run the covariances keep getting smaller.
The Kalman Gain is finally used to compensate for the error between the estimates of the measurements and the actual measurements that are coming in from the camera.
Again, this elegant technique first ensures that the error in the quaternion values converge around zero.
Q2) EKF is a great technique to use as long as you have a non-linear measurement construction technique. Be very careful in using EKF if their are too many transformations in your system, i.e don't try to reconstruct measurements using transformation on your states, this seriously affects the model sanctity and since noise covariances would not undergo similar transformations, there would be a chance of hitting singularity as soon as matrices are non-invertible.
You could look at constant gain KF schemes, which would save you from covariance propagation and save substantial computation effort and time. These techniques are quite new and look very promising. They actively absorb P(error covariance), Q(model noise covariance) and R(measurement noise covariance) and work well with EKF schemes.