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.
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.
I have a question about curve fitting, I have many curves like the one in the picture.
X axis : time
Y axis : temperature
Each sample comes out every 30s.
GOAL : predict the value at the end of the transient
What would you do in this situation?
What I am doing is this :
for every new sample I start a new fitting (and so each fitting is independent from the previous one) and check the value of the fitted curve 2 hours (all curves I have set before 2h) after the start of the measurement. If for a number (let's say 5) of subsequent fitting the value in the future stays more or less the same(+-0.2°C) I so assume that the estimation is the right one.
This approach seems to me far too simple and I think I am not exploiting all information. For example the info of the error I am making punctually (e.g. at minute 4:00 I predict and at 4:30 I see that I am doing an error).
In the picture the red part of the curve is excluded (but the real data in the future passes through it). the estimation is the blue one. You see in this case I don't have a good prediction... In general I have also more flat curves.
Based on the comments above, I tried to formulate an answer as no one else is giving some input.
I think your are using a good basic procedure. Better results may be obtained by using a more appropriate fitting curve, which includes all the dominant dynamics, but avoids overfitting of the data. Based on your figure, the simplest form I could think of is:
s + a(1-e^(-t/tau))
with parameters s (the initial temperature), a (amplitude = steady state value) and tau (dominant time constant). As you mentioned yourself, limiting the allowed range for the parameters may avoid overfitting and increase the quality of your estimation.
Using a random high order function, like you are using now, may give good interpolation results, but are dangerous to use for extrapolation, because strange effects may occur outside the fitting region.
Alternatives
Using the error (eg. correcting for the extrapolated error) may be possible, but is tricky and may not always give good results.
Training a neural network to perform the estimation is probably overkill, but may give better results if applied correctly. Note that you need a lot of training data which should be representative for the data for which you will use the neural network later on.
I am using Kinect to get the positions and orientations of each joint, and then I am sending them to Unity. I noticed that there are a lot of "jumps" or fluctuations in the values, for example, sometimes I don't move my hand and in Unity it rotates 180 degrees.
What I want is a good way to smooth this fluctuations. I heard about the Kalman filter and I implement the code written here
http://www.dyadica.co.uk/very-simple-kalman-in-c/
And it is not bad for the positions but for the orientations is not so good... If you know better approaches or a better way to implement Kalman it would be nice.
On the prior firstly you need to check how well your sensor is able to pick up your variations and movements.
If sensor is a good one, Then Kalman filter would be a good way to start with for removing the jitters and other noise. By looking at your code, you have implemented a one dimensional KF which is fine. But in your case your requirements seems to look like you need proper orientations and positions for which you may have to design a multi-dimensional KF(equations in a matrix format to remove noise in multi-dimension). You will get a better understanding of KF by these links
http://www.codeproject.com/Articles/342099/EMGU-Kalman-Filter
http://www.codeproject.com/Articles/865935/Object-Tracking-Kalman-Filter-with-Ease
Try to implement multi dimension KF and see how well your system responds to it. If you are not satisfied with the performance of your system, Then you may have to extend the filter by making some changes. In the recent past there are some other variants of KF that has came into existence which are Extended KF and Unscented KF . Kalman Filter fails in some practical scenarios where
Noise is not Gaussian zero mean
Input signal from the sensor is non-linear(obvious in practical)
In practical scenarios noise is never zero mean and input is not linear. For this purpose the extension of KF has been introduced. You can go through Extended kalman filter and unscented kalman filter which overcomes the above drawbacks. Both the algorithms are improvements of KF which works on practical cases and can be understandable only if you have some idea on KF.
I currently work in an experimental rock mechanics lab, and when I conduct an experiment, I record the output signals such as effective torque, normal force and motor velocity. However, the latter quantity causes significant cross-talk over the recorded channels, and I want to filter this out. Let me give an example:
Here the upper plot is the strong signal (motor velocity), and the lower is an idle signal that is affected by the cross-talk (blue is raw signal, red is median filtered). The idle channel is only recording noise. We see three effects here. When the motor voltage changes:
the amplitude of the noise increases
the idle signal's median shifts
there is a spike that lasts approximately 0.1 seconds
If we zoom in on the first spike that occurs at around 115 seconds, we get the following plot. This does not seem to be your typical delta-function type of spike, but rather some kind of electronic "echo".
I have seen much work on blind source separation through independent component analysis (ICA), but that did not prove to be effective in my situation. However, since I know the shape of the signal that is causing the cross-talk, there may be better ways to include this information. My question is this: is there a filter or a combination of filters that can tackle the effects mentioned above?
As I am a geologist and not an electrician or mathematician, I don't have a proper background for this kind of material, so please bear with me. I write Python, MATLAB and C++ quite well, so suggested algorithms written in any of those languages is preferred (but not required).
The crosstalk you encounter, results from a parasitic transmission line. Just think of your typical FM-receiver - where the wires equal the antennae. These effects include parasitic and inductive coupling, and form an oscillator (which is the reason, why you cannot see, the theoretically ideal delta spike)
I recognize two different approaches:
use a hardware filtering circuit
use a software-implemented filter
ad 1:
depending on the needed bandwith (maximum frequency/rate of change) on the idle channel, you can determine the corner frequency, as well as the required filter-order, for a given rate of suppression
ad 2:
you can implement several types of filters (IIF, FIR) which resemble these circuits.
Additionally, if you are measuring the aggressive signal anyways, you can use the measurement on the idle channel to determine system-parameters for a mathematical model of the crosstalk. With this model you'd be able, to exclude the interference by calculation
Imagine I have a rectangular reference value for the position/displacement x and I need to smooth it.
The math for translatoric movements is quite simple:
speed: v = x'
acceleration: a = v' = x''
jerk. j = a' = v'' = x'''
I need to limit all these values. So I thought about using rate limiters in Simulink:
This approach works perfect for ramp signals, as you can see in the following output:
BUT, my reference signals for x are no ramps, they are rectangles/steps. Hence the rate limiters are not working, because the derivatives they get to limit are already infinite and Simulink throws an error. How can I resolve this problem? Is there actually a more elegant way to implement the high order rate-limiters? I guess this approach could be unstable in some cases.
continue reading: related question
Even though it seems absurd, the following approach is working: integration and instant derivation does the trick:
leading to:
More elegant, faster and simpler solutions for the whole smoothing problem are highly appreciated!
It's generally not a good idea to differentiate signals in Simulink because of numerical issues, I would advise to start with the higher order derivatives (e.g. acceleration) and integrate, much more robust numerically. This is what the doc about the derivative block says:
The Derivative block output might be very sensitive to the dynamics of
the entire model. The accuracy of the output signal depends on the
size of the time steps taken in the simulation. Smaller steps allow a
smoother and more accurate output curve from this block. However,
unlike with blocks that have continuous states, the solver does not
take smaller steps when the input to this block changes rapidly.
Depending on the dynamics of the driving signal and model, the output
signal of this block might contain unexpected fluctuations. These
fluctuations are primarily due to the driving signal output and solver
step size.
Because of these sensitivities, structure your models to use
integrators (such as Integrator blocks) instead of Derivative blocks.
Integrator blocks have states that allow solvers to adjust step size
and improve accuracy of the simulation. See Circuit Model for an
example of choosing the best-form mathematical model to avoid using
Derivative blocks in your models.
See also Best-Form Mathematical Models for more details.
I was trying to do something similar. I was looking for a "Smooth Ramp". Here is what I found:
A simpler approach is to combine ramp with a second order lag. Then the signal approachs s-shape. And your derivatives will exist and be smooth as well. Only thing to remember is that the 2nd or lag must be critically damped.
Y(s) = H(s)*X(s) where H(s) = K*wo^2/(s^2 + 2*zeta*wo*s + wo^2). Here you define zeta = 1.0. Then the s-shape is retained for any K and wo value. Note that X(s) has already been hit by a ramp. In matlab or any other tools, linear ramp and 2nd lag are standard blocks.
Good luck!
I think the 'Transfer Fcn' block is what you're looking for.
If you leave the equation in the default form 1/(s+1) you have a low-pass filter which can be tuned to what you need by changing the numerator and denominator coefficients.