I've spent a few hours doing research on numeric integration and velocity/position estimation but I couldn't really find an answer that would be either understandable by my brain or appropriate for my situation.
I have an IMU (Inertial Measurement Unit) that has a gyro, an accelerometer and a magnetometer. All those sensors are in fusion, which means for example using the gyro I'm able to compensate for the gravity in the accelerometer readings and the magnetometer compensates the drift.
In other words, I can get pure acceleration readings using such setup.
Now, I'm trying to accurately estimate the position based on acceleration, which as you may know requires double integration, and there are various methods of doing that. But I don't know which would be the most appropriate here.
Could somebody please share some information about this?
Also, I'd appreciate it if you could explain it to me without using any complex math formulas/symbols, I'm not a mathematician and this was one of my problems when looking for information.
Thanks
You can integrate the accelerations by simply summing the acceleration vectors multiplied by the timestep (period of the IMU) to get the velocity, then sum the velocities times the timestep to get the position. You can propagate (not integrate) the orientation by using various methods depending on which orientation representation you choose (Euler angles, Quaternions, Attitude Matrix (DCM), Axis-Angle, etc..).
However you have a bigger problem.
Long story short: Unless you have a naval-quality IMU (USD$200,000+) you cannot simply integrate the accelerations and angular rates to get an accurate pose (position and orientation) estimates.
I assume you are using a low-cost (under USD$1,000) IMU - your accelerometer and gyro are subject to both noise and bias. These will make it impossible to get accurate pose by simply integrating.
In practice to do what you are intending it is required to fuse 'correcting' measurements of the position and optionally the orientation. The IMU 'predicts' the position/orientation, while another sensor model (camera features, gps, altimeter, range/bearing measurements) take the predicted position and 'correct' it. There are various methods of fusing this data, the most prolific of which is the Extended Kalman Filter or Error-State (Indirect) Kalman Filter.
Back to your original question; I would represent the orientation as quaternions, and you can propagate the quaternion orientation by using the error-quaternion derivative and the angular rates from your gyro.
EDIT:
The noise problem can be partially worked around by using a high pass
filter, but what kind of bias are you exactly talking about?
You should read up on the sources of error in MEMS accelerometers: constant alignment bias, random walk bias, white noise and temperature bias. As you said you can high-pass filter to reduce the effect of noise - however this is not perfect so there is significant residual noise. The double integration of the residual noise gives a quadratically-increasing position error. Even after removing the acceleration due to gravity there will be significant accelerations measured due to these error sources which will render the position estimate inaccurate within less than 1 second of integration.
Related
I have IMU sensor that gives me the raw data such as orientation, Angular and Linear acceleration. Im using ROS and doing some Gazebo UUV simulation. Furthermore, I want to get linear velocity from the raw IMU data. If I do integration over time there will be accumulated error and will not be accurate with the time when for example the robot makes turns.
So If I use
acceleration_x = (msg->linear_acceleration.x + 9.81 * sin(pitch)) * cos(pitch);
acceleration_y = (msg->linear_acceleration.y - 9.81 * sin(roll)) * cos(roll);
So integrating linear acceleration is very bad,
Velocity_x= Velocity_old_x+acceleration_x*dt;
because integrates the acceleration without taking into account any possible rotation of the sensor, which means that the results will probably be terrible if the sensor rotates at all.
So I need some ROS package that takes into account all this transformation and gives me the most accurate estimation of the linear velocity. Any Help? Thanks
I would first recommend that you try fitting your input sensor data into an EKF or UKF node from the robot_localization package. This package is the most used & most optimized pose estimation package in the ROS ecosystem.
It is designed to handle 3D sensor input, but you would have to configure the parameters (there are no real defaults, all config). Besides the configuration docs above, the github has good examples of yaml parameter configurations (Ex.) (you'll want a separate file from the launch file) and example launch files (Ex.).
If you're talking about minimizing accumulated error, feeding IMU or odometry-velocity data into an EKF/UKF will give you the odom->base_link frame transform, and that is the best you can do, by definition. Absolute pose error will creep in and accumulate, unless you have an absolute reference frame measurement. (Ex GPS or camera/lidar processed position estimate). Specifc to how you asked for velocity, stepping back a derivative, unless you have an absolute reference frame estimate for your velocity or pose, you will have accumulated error just integrating your acceleration, and that is the best you can do, by definition.
If it's an underwater robot, you may be able to get a velocity / water flow speed sensor attached to your vehicle. Or you may be able to use camera / lidar / sonar with processing to get an absolute reference frame or at least a position difference between execution cycles. Otherwise your precision & results are limited to the sensors you have.
I notice that resonance audio envelope the listener by HOA, I am curious about how do you deal with forbiden frequency (non-uniqueness) in HOA?
HOA describes sound fields accurately only up to a certain frequency and only within a limited region. This is because the spherical Bessel functions only vanish for small wavenumber * radius values. For higher values, indeed, they can be null and the sound field reconstruction will suffer from spatial aliasing errors.
To deal with this problem, I think it is a common practice to apply frequency-dependent smoothing functions in the spherical harmonics domain. For example, instead of trying to reconstruct a high-frequency sound field, we can apply 'MaxRe' weighting factors to form a different kind of an ambisonic 'panner', which will assure best energy concentration in the direction of the virtual source.
I'm working on a video stabilizer using Opencv in C++.
At this time of the project I'm correctly able to find the translation between two consecutive frames with 3 different technique (Optical Flow, Phase Correlation, BFMatcher on points of interests).
To obtain a stabilized image I add up all the translation vector (from consecutive frame) to one, which is used in warpAffine function to correct the output image.
I'm having good result on fixed camera but result on camera in translation are really bad : the image disappear from the screen.
I think I have to distinguish jitter movement that I want to remove from panning movement that I want to keep. But I'm open to others solutions.
Actually the whole problem is a bit more complex than you might have thought in the beginning. Let's look a it this way: when you move your camera through the world, things that move close to the camera move faster than the ones in the background - so objects at different depths change their relative distance (look at your finder while moving the head and see how it points to different things). This means the image actually transforms and does not only translate (move in x or y) - so how do you want to accompensate for that? What you you need to do is to infer how much the camera moved (translation along x,y and z) and how much it rotated (with the angles of yaw, pan and tilt). This is a not very trivial task but openCV comes with a very nice package: http://opencv.willowgarage.com/documentation/camera_calibration_and_3d_reconstruction.html
So I recommend you to read as much on Homography(http://en.wikipedia.org/wiki/Homography), camera models and calibration as possible and then think what you actually want to stabilize for and if it is only for the rotation angles, the task is much simpler than if you would also like to stabilize for translational jitters.
If you don't want to go fancy and neglect the third dimension, I suggest that you average the optic flow, high-pass filter it and compensate this movement with a image translation into the oposite direction. This will keep your image more or less in the middle of the frame and only small,fast changes will be counteracted.
I would suggest you the possible approaches (in complexity order):
apply some easy-to-implement IIR low pass filter on the translation vectors before applying the stabilization. This will separate the high frequency (jitter) from the low frequency (panning)
same idea, a bit more complex, use Kalman filtering to track a motion with constant velocity or acceleration. You can use OpenCV's Kalman filter for that.
A bit more tricky, put a threshold on the motion amplitude to decide between two states (moving vs static camera) and filter the translation or not.
Finaly, you can use some elaborate technique from machine Learning to try to identify the user's desired motion (static, panning, etc.) and filter or not the motion vectors used for the stabilization.
Just a threshold is not a low pass filter.
Possible low pass filters (that are easy to implement):
there is the well known averaging, that is already a low-pass filter whose cutoff frequency depends on the number of samples that go into the averaging equation (the more samples the lower the cutoff frequency).
One frequently used filter is the exponential filter (because it forgets the past with an exponential rate decay). It is simply computed as x_filt(k) = a*x_nofilt(k) + (1-a)x_filt(k-1) with 0 <= a <= 1.
Another popular filter (and that can be computed beyond order 1) is the Butterworth filter.
Etc Low pass filters on Wikipedia, IIR filters...
100 periods have been collected from a 3 dimensional periodic signal. The wavelength slightly varies. The noise of the wavelength follows Gaussian distribution with zero mean. A good estimate of the wavelength is known, that is not an issue here. The noise of the amplitude may not be Gaussian and may be contaminated with outliers.
How can I compute a single period that approximates 'best' all of the collected 100 periods?
Time-series, ARMA, ARIMA, Kalman Filter, autoregression and autocorrelation seem to be keywords here.
UPDATE 1: I have no idea how time-series models work. Are they prepared for varying wavelengths? Can they handle non-smooth true signals? If a time-series model is fitted, can I compute a 'best estimate' for a single period? How?
UPDATE 2: A related question is this. Speed is not an issue in my case. Processing is done off-line, after all periods have been collected.
Origin of the problem: I am measuring acceleration during human steps at 200 Hz. After that I am trying to double integrate the data to get the vertical displacement of the center of gravity. Of course the noise introduces a HUGE error when you integrate twice. I would like to exploit periodicity to reduce this noise. Here is a crude graph of the actual data (y: acceleration in g, x: time in second) of 6 steps corresponding to 3 periods (1 left and 1 right step is a period):
My interest is now purely theoretical, as http://jap.physiology.org/content/39/1/174.abstract gives a pretty good recipe what to do.
We have used wavelets for noise suppression with similar signal measured from cows during walking.
I'm don't think the noise is so much of a problem here and the biggest peaks represent actual changes in the acceleration during walking.
I suppose that the angle of the leg and thus accelerometer changes during your experiment and you need to account for that in order to calculate the distance i.e you need to know what is the orientation of the accelerometer in each time step. See e.g this technical note for one to account for angle.
If you need get accurate measures of the position the best solution would be to get an accelerometer with a magnetometer, which also measures orientation. Something like this should work: http://www.sparkfun.com/products/10321.
EDIT: I have looked into this a bit more in the last few days because a similar project is in my to do list as well... We have not used gyros in the past, but we are doing so in the next project.
The inaccuracy in the positioning doesn't come from the white noise, but from the inaccuracy and drift of the gyro. And the error then accumulates very quickly due to the double integration. Intersense has a product called Navshoe, that addresses this problem by zeroing the error after each step (see this paper). And this is a good introduction to inertial navigation.
Periodic signal without noise has the following property:
f(a) = f(a+k), where k is the wavelength.
Next bit of information that is needed is that your signal is composed of separate samples. Every bit of information you've collected are based on samples, which are values of f() function. From 100 samples, you can get the mean value:
1/n * sum(s_i), where i is in range [0..n-1] and n = 100.
This needs to be done for every dimension of your data. If you use 3d data, it will be applied 3 times. Result would be (x,y,z) points. You can find value of s_i from the periodic signal equation simply by doing
s_i(a).x = f(a+k*i).x
s_i(a).y = f(a+k*i).y
s_i(a).z = f(a+k*i).z
If the wavelength is not accurate, this will give you additional source of error or you'll need to adjust it to match the real wavelength of each period. Since
k*i = k+k+...+k
if the wavelength varies, you'll need to use
k_1+k_2+k_3+...+k_i
instead of k*i.
Unfortunately with errors in wavelength, there will be big problems keeping this k_1..k_i chain in sync with the actual data. You'd actually need to know how to regognize the starting position of each period from your actual data. Possibly need to mark them by hand.
Now, all the mean values you calculated would be functions like this:
m(a) :: R->(x,y,z)
Now this is a curve in 3d space. More complex error models will be left as an excersize for the reader.
If you have a copy of Curve Fitting Toolbox, localized regression might be a good choice.
Curve Fitting Toolbox supports both lowess and loess localized regression models for curve and curve fitting.
There is an option for robust localized regression
The following blog post shows how to use cross validation to estimate an optimzal spaning parameter for a localized regression model, as well as techniques to estimate confidence intervals using a bootstrap.
http://blogs.mathworks.com/loren/2011/01/13/data-driven-fitting/
I'm doing some work on tracking moving objects using a ceiling mounted downward facing camera. I've got to the point where I can detect the position of the desired object in each frame.
I'm looking into using a Kalman filter to track the object's position and speed through the scene and I've reached a stumbling block. I've set up my system and have all the required parts of the Kalman filter except the measurement variance.
I want to be able to assign a meaningful variance to each measurement to allow the correction phase to use the new information in a sensible manner. I have several measures assigned to my detected objects which could in theory be useful in determining how accurate the position should be and it seems logical to try and combine them to derive a suitable variance.
Am I approaching this in the right manner and if so, can anyone point me in the right direction to continue?
Any help greatly appreciated.
I think you are right. According to this post:
Sensor fusioning with Kalman filter
determining the variance is 100% experimental. It seems to me you have everything you need to get good estimates of the variance.
sorry for the late reply. I have personally encountered the same problem in my previous project. I found the advice given by Gustaf Hendeby in his Sensor Fusion lecture slides ( Page 10 of the slides) extremely valuable.
To summarize:
(1) The SNR of your measurement noise and your process noise determines your filter behavior. A high process noise/measurement noise ration makes your filter slower (low-pass filter), which will usually allow smoother tracking, vice versa a if you set your measurement noise low, you essentially have a high pass filter, which tends to have more jitter.
(2) There are numerous papers in the literature discuss on how to set these noise model properly. However, usually a lot of "tuning" is needed depends on your application. Usually the measurement noise is what we can measure/characterize based on the hardware specification. Therefore a recommendation is to fix "R" (measurement noise covariance) and tune Q (the process model noise covariance).