I'm interested, how is the dual input in a sensor fusioning setup in a Kalman filter modeled?
Say for instance that you have an accelerometer and a gyro and want to present the "horizon level", like in an airplane, a good demo of something like this here.
How do you actually harvest the two sensors positive properties and minimize the negative?
Is this modeled in the Observation Model matrix (usually symbolized by capital H)?
Remark: This question was also asked without any answers at math.stackexchange.com
Usually, the sensor fusion problem is derived from the bayes theorem. Actually you have that your estimate (in this case the horizon level) will be a weighted sum of your sensors, which is caracterized by the sensor model. For dual sensors, you have two common choices: Model a two sensor system and derive the kalman gain for each sensor (using the system model as the predictor), or run two correction stages using different observation models. You should take a look at Bayesian Predictors (a little more general than Kalman Filter) which is precisely derived from minimizing the variance of an estimate, given two different information sources. If you have a weighted sum, and minimize the variance of the sum, for two sensors, then you get the Kalman Gain.
The properties of the sensor can be "seen" in two parts of the filter. First, you have the error matrix for your observations. This is the matrix that represents the noise in the sensors observation (it is assumed to be zero mean gaussian noise, which isn't a too big assumption, given that during calibration, you can achieve a zero mean noise).
The other important matrix is the observation covariance matrix. This matrix gives you an insight about how good is the sensor at giving you information (information meaning something "new" and not dependent on the other sensors reading).
About "harvesting the good characteristics", what you should do is do a good calibration and noise characterization (is that spelled ok?) of the sensors. The best way to get a Kalman Filter to converge is to have a good noise model for your sensors, and that is 100% experimental. Try to determine the variance for your system (dont always trust datasheets).
Hope that helps a bit.
The gyro measures rate of angle change (e.g. in radians per sec), while from accelerometer reading you can calculate the angle itself. Here is a simple way of combining these measurements:
At every gyro reading received:
angle_radians+=gyro_reading_radians_per_sec * seconds_since_last_gyro_reading
At every accelerometer reading received:
angle_radians+=0.02 * (angle_radians_from_accelerometer - angle_radians)
The 0.02 constant is for tuning - it selects the tradeoff between noise rejection and responsiveness (you can't have both at the same time). It also depends on the accuracy of both sensors, and the time intervals at which new readings are received.
These two lines of code implement a simple 1-dimensional (scalar) Kalman filter. It assumes that
the gyro has very low noise compared to accelerometer (true with most consumer-grade sensors). Therefore we do not model gyro noise at all, but instead use gyro in the state transition model (usually denoted by F).
accelerometer readings are received at generally regular time intervals and accelerometer noise level (usually R) is constant
angle_radians has been initialised with an initial estimate (f.ex by averaging angle_radians_from_accelerometer over some time)
therefore also estimate covariance (P) and optimal Kalman gain (K) are constant, which means we do not need to keep estimate covariance in a variable at all.
As you see, this approach is simplified. If the above assumptions are not met, you should learn some Kalman filter theory, and modify the code accordingly.
Horizon line is G' * (u, v, f)=0 ,where G is a gravity vector, u and v image centred coordinates and f focal length. Now pros and cons of sensors: gyro is super fast and accurate but drifts, accelerometer is less accurate but (if calibrated) has zero bias and doesn't drift (given no acceleration except gravity). They measure different things - accelerometer measures acceleration and thus orientation relative to the gravity vector while gyro measures rotation speed and thus the change in orientation. To convert it to orientation one has to integrate its values (thankfully it can be sampled at high fps like 100-200). thus Kalman filter that supposed to be linear is not applicable to gyro. for now we can just simplify sensor fusion as a weighted sum of readings and predictions.
You can combine two readings - accelerometer and integrated gyro and model prediction using weights that are inversely proportional to data variances. You will also have to use compass occasionally since accelerometer doesn't tell you much about the azimuth but I guess it is irrelevant for calculation of a horizon line. The system should be responsive and accurate and for this purpose whenever orientation changes fast the weights for gyro should be large; when the system settles down and rotation stops the weights for accelerometer will go up allowing more integration of zero bias readings and killing the drift from gyro.
Related
I am analyzing a voltage output that I get from spice simulator and I want to quantize the time sampled voltage data so that I can convert the given (trapezoidal wave) to a square wave.
I have tried differentiation as a method to understand when the data is at flat level(one of the voltage levels) or when it is in transition, but since there are inherent glitches in the voltage vs time data I am not able to get the exact levels that I want. The waveform extracted is from simulator and I am doing all the analysis in "C" language.
Voltage waveform with a glitch
There are a number of methods to deal with this kind of a problem, but a nice solution to this will be the following:
Convolve the signal with a gaussian kernel, thus canceling out the noise at the cost of making the transitions between the levels slower
Due to the convolution, the transition will not be linear but there will be a place in the transition where the curvature changes sign, which is the location of the original edge. To find these locations, compute the Laplacian, which is simply the second derivative in 1D and look for places the Laplacian crosses 0.
To avoid the undershoot and overshoot, take a buffer of a few samples from the found edges and compute the mean, thus finding the height of the level
Steps 1 and 2 can be united if you convolve with the Laplacian of a gaussian kernel. A very good explanation for this edge detection algorithm is shown here, explained by Shree K. Nayar
This question is not directly related to a particular programming language but is an algorithmic question.
What I have is a lot of samples of a 2D function. The samples are at random locations, they are not uniformly distributed over the domain, the sample values contain noise and each sample has a confidence-weight assigned to it.
What I'm looking for is an algorithm to reconstruct the original 2D function based on the samples, so a function y' = G(x0, x1) that approximates the original well and interpolates areas where samples are sparse smoothly.
It goes into the direction of what scipy.interpolate.griddata is doing, but with the added difficulty that:
the sample values contain noise - meaning that samples should not just be interpolated, but nearby samples also averaged in some way to average out the sampling noise.
the samples are weighted, so, samples with higher weight should contrbute more strongly to the reconstruction that those with lower weight.
scipy.interpolate.griddata seems to do a Delaunay triangulation and then use the barycentric cordinates of the triangles to interpolate values. This doesn't seem to be compatible with my requirement of weighting samples and averaging noise though.
Can someone point me in the right direction on how to solve this?
Based on the comments, the function is defined on a sphere. That simplifies life because your region is both well-studied and nicely bounded!
First, decide how many Spherical Harmonic functions you will use in your approximation. The fewer you use, the more you smooth out noise. The more you use, the more accurate it will be. But if you use any of a particular degree, you should use all of them.
And now you just impose the condition that the sum of the squares of the weighted errors should be minimized. That will lead to a system of linear equations, which you then solve to get the coefficients of each harmonic function.
I implemented a bootstrap Particle filter on C++ by reading few Papers and I first implemented a 1D mouse tracker which performed really well. I used normal Gaussian for weighting in this exam.
I extended the algorithm to track face using 2 features of Local motion and HSV 32 bin Histogram. In this example my weighing function becomes the probability of Motion x probability of Histogram. (Is this correct).
Incase if that is correct than I am confused on the resampling function. At the moment my resampling function is as follows:
For each Particle N = 50;
Compute CDF
Generate a random number (via Gaussian) X
Update the particle at index X
Repeat for all N particles.
This is my re-sampling function at the moment. Note: the second step I am using a Random Number via Gaussian distribution for get the index while my weighting function is Probability of Motion and Histogram.
My question is: Should I generate random number using the probability of Motion and Histogram or just the random number via Gaussian is ok.
In the SIR (Sequential Importance Resampling) particle filter, resampling aims to replicate particles that have gained high weight, while remove those with less weight.
So, when you have your particles weighted (typically with the likelihood you have used), one way to do resampling is to create the cumulative distribution of the weights, and then generate a random number following a uniform distribution and pick the particle corresponding to the slot of the CDF. This way there is more probability to select a particle that has more weight.
Also, don't forget to add some noise after generating replicas of particles, otherwise your point-estimate might be biased for a period of time.
So for this wind monitoring project I'm getting data from a couple of 3d sonic anemometers, specifically 2 R.M.Young 81000. The data output is made digitally with a sampling frequency of 10Hz for periods of 10min. After all the pre-processing (coordinate rotation, trend removal...) I get 3 orthogonal time series of the turbulent data. Right now I'm using the stationary data of 2 hours of measurements with windows of 4096 points and a 50% overlapping to obtain the frequency spectrums in all three directions. After obtaining the spectrum I apply a logarithmic frequency smoothing algorithm, which averages the obtained spectrum in logarithmic spaced intervals.
I have two questions:
The spectrums I obtain from the measured show a clear downward trend in the highest frequencies as seen in the attached figure. I wonder if this loss of energy can have anything to do with an internal filter from the sonic anemometer? Or what else? Is there a way to compensate this loss or better just to consider the spectrum until the "break frequency"?
http://i.stack.imgur.com/B11uP.png
When applying the curve fitting algorithm to determine the integral length scales according to the von Karman equation what is the correct procedure: curve fitting the original data, which gives more weight to higher frequency data points? or using the logarithmic frequency smoothed data to approximate the von karman equation, giving an equal weight to data in the logarithmic scale? In some cases I obtain very different estimates for the integral length scales using both approaches (ex: Original -> Lu=113.16 Lv=42.68 Lw=9.23; Freq. Smoothed -> Lu=148.60 Lv=30.91 Lw=14.13).
Curve fitting with Logarithmic frequency smoothing and with Original data:
http://i.imgur.com/VL2cf.png
Let me know if something is not clear. I'm relatively new in this field, and I might me be making some mistakes in my approach, so if you could give me some advice or tips it would be amazing.
Suppose to have a GPS trajectory - i.e.: a series of spatio-temporal coords, every coord is a (x,y,t) information, where x is longitude, y is latitude and t is the time stamp.
Suppose each trajectory identified by 1000 (x,y) points, a compressed trajectory is trajectory with fewer points than the original, for instance 300 points. A compression algorithm (Douglas-Peucker, Bellman, etc) decide what points will be in compressed trajectory and what point will be discarded.
Each algorithm make his own choice. Better algorithms choice the points not only by spatial characteristics (x, y) but using spatio-temporal characteristics (x,y,t).
Now I need a way to compare two compressed trajectories against the original to understand what compression algorithm better reduce a spatio-temporal (temporal component is really important) trajectory.
I've thinked of DTW algorithm to check trajectory similarity, but this probably don't care about temporal component. What algorithm can I use to make this control?
What is the best compression algorithm depends to a large extent on what you are trying to achieve with it, and is dependent on other external variables. Typically, were going to identify and remove spikes, and then remove redundant data. For example;
Known minimum and maximum velocity, acceleration, and ability to tuen will let you remove spikes. If we look at the join distance between a pair of points divided by the time where
velocity = sqrt((xb - xa)^2 + (yb - ya))/(tb-ta)
we can eliminate points where the distance couldn't be travelled in the elapsed time given the speed constraint. We can do the same with acceleration constraints, and change in direction constraints for a given velocity. These constraints change whether the GPS receiver is static, hand held, in a car, in an aeroplane etc...
We can remove redundant points using a moving window looking at three points, where if the an interpolated (x,y,t) for middle point can be compared with an observed point, and the observed point removed if it lies within a specified distance + time tolerance of the interpolated point. We can also curve fit the data and consider the distance to the curve rather than using a moving 3 point window.
The compression may also have differing goals based on the constraints given, e.g. to simply reduce the data size by removing redundant observations and spikes, or to smooth the data as well.
For the former, after checking for spikes based on defined constraints, we simply check the 3d distance of each point to the polyline connecting the compressed points. This is achieved by finding the pair of points before and after the point that has been removed, interpolating a position on the line connecting those points based on the observed time, and comparing the interpolated position with the observed position. The amount of points removed will increase as we allow this distance tolerance to increase.
For the latter we also have to consider how well the smoothed result models the data, the weights imposed by the constraints, and the design shape / curve parameters.
Hope this makes some sense.
Maybe you could use mean square distance between trajectories over time.
Probably, simply looking at distance at time 1s,2s,... will be enough, but you can also do it more precise between time stamps integrating, (x1(t)-x2(t))^2 + (y1(t)-y2(t))^2. Note that between 2 time stamps both trajectories will be straight line.
I've found what I need to compute spatio-temporal error.
As written in paper "Compression and Mining of GPS Trace Data:
New Techniques and Applications" by Lawson, Ravi & Hwang:
Synchronized Euclidean distance (sed) measures the distance between
two points at identical time stamps. In Figure 1, five time steps (t1
through t5) are shown. The simplified line (which can be thought of as
the compressed representation of the trace) is comprised of only two
points (P't1 and P't5); thereby, it does not include points P't2, P't3
and P't4. To quantify the error introduced by these missing points,
distance is measured at the identical time steps. Since three points
were removed between P't1 and P't5, the line is divided into four
equal sized line segments using the three points P't2, P't3 and P't4
for the purposes of measuring the error. The total error is measured
as the sum of the distance between all points at the synchronized time
instants, as shown below. (In the following expression, n represents
the total number of points considered.)