I am working on a object tracking project and I want to improve the results I am getting using a Kalman filter.
I have found a lot of examples on the internet which are working but I really want to understand what is behind it.
Using opencv, here is a part of the code :
KalmanFilter KF(6, 2, 0);
Mat_ state(6, 1);
Mat processNoise(6, 1, CV_32F);
...
KF.statePre.at(0) = mouse_info.x;
KF.statePre.at(1) = mouse_info.y;
KF.statePre.at(2) = 0;
KF.statePre.at(3) = 0;
KF.statePre.at(4) = 0;
KF.statePre.at(5) = 0;
KF.transitionMatrix = *(Mat_(6, 6) << 1,0,1,0,0.5,0, 0,1,0,1,0,0.5, 0,0,1,0,1,0, 0,0,0,1,0,1, 0,0,0,0,1,0, 0,0,0,0,0,1);
KF.measurementMatrix = *(Mat_(2, 6) << 1,0,1,0,0.5,0, 0,1,0,1,0,0.5);
This one gives smoother results than a KalmanFilter(4,2,0) but I don't really understand why.
Can someone explain me what is behind this (6,6) transition matrix ?
EDIT : The solution is probably here but obviously I am not good enough to find it by myself ...
Thank you for your help.
You have a state vector X made up of 6 components, the first two of which are the x and y position of an object; let's assume that the other 4 are their velocities and accelerations:
X = [x, y, v_x, v_y, a_x, a_y] t
In the Kalman filter, your next state, Xt+1, is equal to the previous state Xt multiplied by the transition matrix A, so with the transition matrix you posted, you would have:
x t+1 = x t + v_x t + 0.5 a_x t
y t+1 = y t + v_y t + 0.5 a_y t
v_x t+1 = v_x t + a_x t
v_y t+1 = v_t t + a_t t
a_x t+1 = a_x t
a_y t+1 = a_y t
Which are the discrete approximation of the equations of an object moving with constant acceleration if the time interval between the two states is equal to 1 (and that's why it makes sense to suppose that the other four variables are velocities and accelerations).
This is a Kalman filter that allows for faster variations in the velocity estimation, so it introduces a lower delay than a (4, 2, 0) filter, which would use a constant velocity model.
Related
I'm writing a function that takes in an object with a trajectory (including starting position, starting velocity, and acceleration, all represented as Vector3s) in 3D space and if it hits another object, returns the point of collision and time of the collision. I'm using kinematic equations with a timestep to detect possible collisions and I can get the point of collision that way, but once I have that I want to find the exact time that that collision would occur at.I thought of rearranging a kinematic equation to solve for time and plug in what I already had, but I can't figure out how I can use all three axes of motion to do this, since my other values are Vec3's and time is just scalar. I've thought about just doing the calculation on one axis, but I'm not sure if that would lead to an accurate result.
Would it be accurate to calculate just based on one axis, or is there a way to incorporate all three into the calculation? The formula I'm using to solve for time is:
t = (v_init +/- Sqrt((v_init)^2 - (accel * disp * 4 * .5)))/accel;
Where v_init is initial velocity, disp is total displacement, and accel is acceleration. I'm basing this off of the kinematic equation:
d = v*t + .5*a*t^2
Let me write in the general case. The component-wise motion law is
x(t) = x0 + v_x t + 0.5 a_x t^2
y(t) = y0 + v_y t + 0.5 a_y t^2
z(t) = z0 + v_z t + 0.5 a_z t^2
where (x0,y0,z0)^t is the initial position, (v_x, v_y, v_z)^t is the initial velocity vector, and (a_x, a_y, a_z)^t is the vector of acceleration. The 3rd component of the latter may include also the gravity acceleration.
I assume that the collision plane is horizontal, having thus equation z = k. Solve in t the equation
z(t) = k
for finding the time t_c in which the projectile hits the plane. Compute then the collision coordinates x(t_c) and y(t_c) using the above formula by substituting t with t_c.
If the plane has the general equation
a x + b y +c z + d = 0
I suggest to put the frame of reference on the plane, having the xy plane on the collision plane, and then apply the above procedure.
You may also solve the non linear system
x = x0 + v_x t + 0.5 a_x t^2
y = y0 + v_y t + 0.5 a_y t^2
z = z0 + v_z t + 0.5 a_z t^2
a x + b y +c z + d = 0
taking the solution for t>0 (I dropped the dependency on t for x, y and z).
To solve it in C++, you may search a math library, such as Eigen which has a module for non linear systems.
I am reverse engineering a game from 1999 and I came across a function which looks to be checking if the player is within range of a 3d point for the triggering of audio sources. The decompiler mangles the code pretty bad but I think I understand it.
// Position Y delta
v1 = * (float * )(this + 16) - LocalPlayerZoneEntry - > y;
// Position X delta
v2 = * (float * )(this + 20) - LocalPlayerZoneEntry - > x;
// Absolute value
if (v1 < 0.0)
v1 = -v1;
// Absolute value
if (v2 < 0.0)
v2 = -v2;
// What is going on here?
if (v1 <= v2)
v1 = v1 * 0.5;
else
v2 = v2 * 0.5;
// Z position delta
v3 = * (float * )(this + 24) - LocalPlayerZoneEntry - > z;
// Absolute value
if (v3 < 0.0)
v3 = -v3;
result = v3 + v2 + v1;
// Radius
if (result > * (float * )(this + 28))
return 0.0;
return result;
Interestingly enough, when in game, it seemed like the triggering was pretty inconsistent and would sometimes be quite a bit off depending on from which side I approached the trigger.
Does anyone have any idea if this was a common algorithm used back in the day?
Note: The types were all added by me so they may be incorrect. I assume that this is a function of type bool.
The best way to visualize a distance function (a metric) is to plot its unit sphere (the set of points at unit distance from origin -- the metric in question is norm induced).
First rewrite it in a more mathematical form:
N(x,y,z) = 0.5*|x| + |y| + |z| when |x| <= |y|
= |x| + 0.5*|y| + |z| otherwise
Let's do that for 2d (assume that z = 0). The absolute values make the function symmetric in the four quadrants. The |x| <= |y| condition makes it symmetric in all the eight sectors. Let's focus on the sector x > 0, y > 0, x <= y. We want to find the curve when N(x,y,0) = 1. For that sector it reduces to 0.5x + y = 1, or y = 1 - 0.5x. We can go and plot that line. For when x > 0, y > 0, x > y, we get x = 1 - 0.5y. Plotting it all gives the following unit 'circle':
For comparison, here is an Euclidean unit circle overlaid:
In the third dimension it behaves like a taxicab metric, effectively giving you a 'diamond' shaped sphere:
So yes, it is a cheap distance function, though it lacks rotational symmetries.
I would like to fit a 2D array by an elliptic function: (x / a)² + (y / b)² = 1 ----> (and so get the a and b)
And then, be able to replot it on my graph.
I found many examples on internet, but no one with this simple Cartesian equation. I probably have searched badly ! I think a basic solution for this problem could help many people.
Here is an example of the data:
Sadly, I can not put the values... So let's assume that I have an X,Y arrays defining the coordinates of each of those points.
This can be solved directly using least squares. You can frame this as minimizing the sum of squares of quantity (alpha * x_i^2 + beta * y_i^2 - 1) where alpha is 1/a^2 and beta is 1/b^2. You have all the x_i's in X and the y_i's in Y so you can find the minimizer of ||Ax - b||^2 where A is an Nx2 matrix (i.e. [X^2, Y^2]), x is the column vector [alpha; beta] and b is column vector of all ones.
The following code solves the more general problem for an ellipse of the form Ax^2 + Bxy + Cy^2 + Dx +Ey = 1 though the idea is exactly the same. The print statement gives 0.0776x^2 + 0.0315xy+0.125y^2+0.00457x+0.00314y = 1 and the image of the ellipse generated is also below
import numpy as np
import matplotlib.pyplot as plt
alpha = 5
beta = 3
N = 500
DIM = 2
np.random.seed(2)
# Generate random points on the unit circle by sampling uniform angles
theta = np.random.uniform(0, 2*np.pi, (N,1))
eps_noise = 0.2 * np.random.normal(size=[N,1])
circle = np.hstack([np.cos(theta), np.sin(theta)])
# Stretch and rotate circle to an ellipse with random linear tranformation
B = np.random.randint(-3, 3, (DIM, DIM))
noisy_ellipse = circle.dot(B) + eps_noise
# Extract x coords and y coords of the ellipse as column vectors
X = noisy_ellipse[:,0:1]
Y = noisy_ellipse[:,1:]
# Formulate and solve the least squares problem ||Ax - b ||^2
A = np.hstack([X**2, X * Y, Y**2, X, Y])
b = np.ones_like(X)
x = np.linalg.lstsq(A, b)[0].squeeze()
# Print the equation of the ellipse in standard form
print('The ellipse is given by {0:.3}x^2 + {1:.3}xy+{2:.3}y^2+{3:.3}x+{4:.3}y = 1'.format(x[0], x[1],x[2],x[3],x[4]))
# Plot the noisy data
plt.scatter(X, Y, label='Data Points')
# Plot the original ellipse from which the data was generated
phi = np.linspace(0, 2*np.pi, 1000).reshape((1000,1))
c = np.hstack([np.cos(phi), np.sin(phi)])
ground_truth_ellipse = c.dot(B)
plt.plot(ground_truth_ellipse[:,0], ground_truth_ellipse[:,1], 'k--', label='Generating Ellipse')
# Plot the least squares ellipse
x_coord = np.linspace(-5,5,300)
y_coord = np.linspace(-5,5,300)
X_coord, Y_coord = np.meshgrid(x_coord, y_coord)
Z_coord = x[0] * X_coord ** 2 + x[1] * X_coord * Y_coord + x[2] * Y_coord**2 + x[3] * X_coord + x[4] * Y_coord
plt.contour(X_coord, Y_coord, Z_coord, levels=[1], colors=('r'), linewidths=2)
plt.legend()
plt.xlabel('X')
plt.ylabel('Y')
plt.show()
Following the suggestion by ErroriSalvo, here is the complete process of fitting an ellipse using the SVD. The arrays x, y are coordinates of the given points, let's say there are N points. Then U, S, V are obtained from the SVD of the centered coordinate array of shape (2, N). So, U is a 2 by 2 orthogonal matrix (rotation), S is a vector of length 2 (singular values), and V, which we do not need, is an N by N orthogonal matrix.
The linear map transforming the unit circle to the ellipse of best fit is
sqrt(2/N) * U * diag(S)
where diag(S) is the diagonal matrix with singular values on the diagonal. To see why the factor of sqrt(2/N) is needed, imagine that the points x, y are taken uniformly from the unit circle. Then sum(x**2) + sum(y**2) is N, and so the coordinate matrix consists of two orthogonal rows of length sqrt(N/2), hence its norm (the largest singular value) is sqrt(N/2). We need to bring this down to 1 to have the unit circle.
N = 300
t = np.linspace(0, 2*np.pi, N)
x = 5*np.cos(t) + 0.2*np.random.normal(size=N) + 1
y = 4*np.sin(t+0.5) + 0.2*np.random.normal(size=N)
plt.plot(x, y, '.') # given points
xmean, ymean = x.mean(), y.mean()
x -= xmean
y -= ymean
U, S, V = np.linalg.svd(np.stack((x, y)))
tt = np.linspace(0, 2*np.pi, 1000)
circle = np.stack((np.cos(tt), np.sin(tt))) # unit circle
transform = np.sqrt(2/N) * U.dot(np.diag(S)) # transformation matrix
fit = transform.dot(circle) + np.array([[xmean], [ymean]])
plt.plot(fit[0, :], fit[1, :], 'r')
plt.show()
But if you assume that there is no rotation, then np.sqrt(2/N) * S is all you need; these are a and b in the equation of the ellipse.
You could try a Singular Value Decomposition of the data matrix.
https://docs.scipy.org/doc/numpy-1.13.0/reference/generated/numpy.linalg.svd.html
First center the data by subtracting mean values of X,Y from each column respectively.
X=X-np.mean(X)
Y=Y-np.mean(Y)
D=np.vstack(X,Y)
Then, apply SVD and extract
-eigenvalues (members of s) -> axis length
-eigenvectors(U) -> axis orientation
U, s, V = np.linalg.svd(D, full_matrices=True)
This should be a least-squares fit.
Of course, things can get more complicated than this, please see
https://www.emis.de/journals/BBMS/Bulletin/sup962/gander.pdf
I'm trying to do a polar transform on the first image below and end up with the second. However my result is the third image. I have a feeling it has to do with what location I choose as my "origin" but am unsure.
radius = sqrt(width**2 + height**2)
nheight = int(ceil(radius)/2)
nwidth = int(ceil(radius/2))
for y in range(0, height):
for x in range(0, width):
t = int(atan(y/x))
r = int(sqrt(x**2+y**2)/2)
color = getColor(getPixel(pic, x, y))
setColor( getPixel(radial,r,t), color)
There are a few differences / errors:
They use the centre of the image as the origin
They scale the axis appropriately. In your example, you're plotting your angle (between 0 and in your case, pi), instead of utilising the full height of the image.
You're using the wrong atan function (atan2 works a lot better in this situation :))
Not amazingly important, but you're rounding unnecessarily quite a lot, which throws off accuracy a little and can slow things down.
This is the code combining my suggested improvements. It's not massively efficient, but it should hopefully work :)
maxradius = sqrt(width**2 + height**2)/2
rscale = width / maxradius
tscale = height / (2*math.pi)
for y in range(0, height):
dy = y - height/2
for x in range(0, width):
dx = x - width/2
t = atan2(dy,dx)%(2*math.pi)
r = sqrt(dx**2+dy**2)
color = getColor(getPixel(pic, x, y))
setColor( getPixel(radial,int(r*rscale),int(t*tscale)), color)
In particular, it fixes the above problems in the following ways:
We use dx = x - width / 2 as a measure of distance from the centre, and similarly with dy. We then use these in replace of x, y throughout the computation.
We will have our r satisfying 0 <= r <= sqrt( (width/2)^2 +(height/2)^2 ), and our t eventually satisfying 0 < t <= 2 pi so, I create the appropriate scale factors to put r and t along the x and y axes respectively.
Normal atan can only distinguish based on gradients, and is computationally unstable near vertical lines... Instead, atan2 (see http://en.wikipedia.org/wiki/Atan2) solves both problems, and accepts (y,x) pairs to give an angle. atan2 returns an angle -pi < t <= pi, so we can find the remainder modulo 2 * math.pi to it to get it in the range 0 < t <= 2pi ready for scaling.
I've only rounded at the end, when the new pixels get set.
Any questions, just ask!
I used to work with MATLAB, and for the question I raised I can use p = polyfit(x,y,1) to estimate the best fit line for the scatter data in a plate. I was wondering which resources I can rely on to implement the line fitting algorithm with C++. I understand there are a lot of algorithms for this subject, and for me I expect the algorithm should be fast and meantime it can obtain the comparable accuracy of polyfit function in MATLAB.
This page describes the algorithm easier than Wikipedia, without extra steps to calculate the means etc. : http://faculty.cs.niu.edu/~hutchins/csci230/best-fit.htm . Almost quoted from there, in C++ it's:
#include <vector>
#include <cmath>
struct Point {
double _x, _y;
};
struct Line {
double _slope, _yInt;
double getYforX(double x) {
return _slope*x + _yInt;
}
// Construct line from points
bool fitPoints(const std::vector<Point> &pts) {
int nPoints = pts.size();
if( nPoints < 2 ) {
// Fail: infinitely many lines passing through this single point
return false;
}
double sumX=0, sumY=0, sumXY=0, sumX2=0;
for(int i=0; i<nPoints; i++) {
sumX += pts[i]._x;
sumY += pts[i]._y;
sumXY += pts[i]._x * pts[i]._y;
sumX2 += pts[i]._x * pts[i]._x;
}
double xMean = sumX / nPoints;
double yMean = sumY / nPoints;
double denominator = sumX2 - sumX * xMean;
// You can tune the eps (1e-7) below for your specific task
if( std::fabs(denominator) < 1e-7 ) {
// Fail: it seems a vertical line
return false;
}
_slope = (sumXY - sumX * yMean) / denominator;
_yInt = yMean - _slope * xMean;
return true;
}
};
Please, be aware that both this algorithm and the algorithm from Wikipedia ( http://en.wikipedia.org/wiki/Simple_linear_regression#Fitting_the_regression_line ) fail in case the "best" description of points is a vertical line. They fail because they use
y = k*x + b
line equation which intrinsically is not capable to describe vertical lines. If you want to cover also the cases when data points are "best" described by vertical lines, you need a line fitting algorithm which uses
A*x + B*y + C = 0
line equation. You can still modify the current algorithm to produce that equation:
y = k*x + b <=>
y - k*x - b = 0 <=>
B=1, A=-k, C=-b
In terms of the above code:
B=1, A=-_slope, C=-_yInt
And in "then" block of the if checking for denominator equal to 0, instead of // Fail: it seems a vertical line, produce the following line equation:
x = xMean <=>
x - xMean = 0 <=>
A=1, B=0, C=-xMean
I've just noticed that the original article I was referring to has been deleted. And this web page proposes a little different formula for line fitting: http://hotmath.com/hotmath_help/topics/line-of-best-fit.html
double denominator = sumX2 - 2 * sumX * xMean + nPoints * xMean * xMean;
...
_slope = (sumXY - sumY*xMean - sumX * yMean + nPoints * xMean * yMean) / denominator;
The formulas are identical because nPoints*xMean == sumX and nPoints*xMean*yMean == sumX * yMean == sumY * xMean.
I would suggest coding it from scratch. It is a very simple implementation in C++. You can code up both the intercept and gradient for least-squares fit (the same method as polyfit) from your data directly from the formulas here
http://en.wikipedia.org/wiki/Simple_linear_regression#Fitting_the_regression_line
These are closed form formulas that you can easily evaluate yourself using loops. If you were using higher degree fits then I would suggest a matrix library or more sophisticated algorithms but for simple linear regression as you describe above this is all you need. Matrices and linear algebra routines would be overkill for such a problem (in my opinion).
Equation of line is Ax + By + C=0.
So it can be easily( when B is not so close to zero ) convert to y = (-A/B)*x + (-C/B)
typedef double scalar_type;
typedef std::array< scalar_type, 2 > point_type;
typedef std::vector< point_type > cloud_type;
bool fit( scalar_type & A, scalar_type & B, scalar_type & C, cloud_type const& cloud )
{
if( cloud.size() < 2 ){ return false; }
scalar_type X=0, Y=0, XY=0, X2=0, Y2=0;
for( auto const& point: cloud )
{ // Do all calculation symmetric regarding X and Y
X += point[0];
Y += point[1];
XY += point[0] * point[1];
X2 += point[0] * point[0];
Y2 += point[1] * point[1];
}
X /= cloud.size();
Y /= cloud.size();
XY /= cloud.size();
X2 /= cloud.size();
Y2 /= cloud.size();
A = - ( XY - X * Y ); //!< Common for both solution
scalar_type Bx = X2 - X * X;
scalar_type By = Y2 - Y * Y;
if( fabs( Bx ) < fabs( By ) ) //!< Test verticality/horizontality
{ // Line is more Vertical.
B = By;
std::swap(A,B);
}
else
{ // Line is more Horizontal.
// Classical solution, when we expect more horizontal-like line
B = Bx;
}
C = - ( A * X + B * Y );
//Optional normalization:
// scalar_type D = sqrt( A*A + B*B );
// A /= D;
// B /= D;
// C /= D;
return true;
}
You can also use or go over this implementation there is also documentation here.
Fitting a Line can be acomplished in different ways.
Least Square means minimizing the sum of the squared distance.
But you could take another cost function as example the (not squared) distance. But normaly you use the squred distance (Least Square).
There is also a possibility to define the distance in different ways. Normaly you just use the "y"-axis for the distance. But you could also use the total/orthogonal distance. There the distance is calculated in x- and y-direction. This can be a better fit if you have also errors in x direction (let it be the time of measurment) and you didn't start the measurment on the exact time you saved in the data. For Least Square and Total Least Square Line fit exist algorithms in closed form. So if you fitted with one of those you will get the line with the minimal sum of the squared distance to the datapoints. You can't fit a better line in the sence of your defenition. You could just change the definition as examples taking another cost function or defining distance in another way.
There is a lot of stuff about fitting models into data you could think of, but normaly they all use the "Least Square Line Fit" and you should be fine most times. But if you have a special case it can be necessary to think about what your doing. Taking Least Square done in maybe a few minutes. Thinking about what Method fits you best to the problem envolves understanding the math, which can take indefinit time :-).
Note: This answer is NOT AN ANSWER TO THIS QUESTION but to this one "Line closest to a set of points" that has been flagged as "duplicate" of this one (incorrectly in my opinion), no way to add new answers to it.
The question asks for:
Find the line whose distance from all the points is minimum ? By
distance I mean the shortest distance between the point and the line.
The most usual interpretation of distance "between the point and the line" is the euclidean distance and the most common interpretation of "from all points" is the sum of distances (in absolute or squared value).
When the target is minimize the sum of squared euclidean distances, the linear regression (LST) is not the algorithm to use. In addition, linear regression can not result in a vertical line. The algorithm to be used is the "total least squares". See by example wikipedia for the problem description and this answer in math stack exchange for details about the formulation.
to fit a line y=param[0]x+param[1] simply do this:
// loop over data:
{
sum_x += x[i];
sum_y += y[i];
sum_xy += x[i] * y[i];
sum_x2 += x[i] * x[i];
}
// means
double mean_x = sum_x / ninliers;
double mean_y = sum_y / ninliers;
float varx = sum_x2 - sum_x * mean_x;
float cov = sum_xy - sum_x * mean_y;
// check for zero varx
param[0] = cov / varx;
param[1] = mean_y - param[0] * mean_x;
More on the topic http://easycalculation.com/statistics/learn-regression.php
(formulas are the same, they just multiplied and divided by N, a sample sz.). If you want to fit plane to 3D data use a similar approach -
http://www.mymathforum.com/viewtopic.php?f=13&t=8793
Disclaimer: all quadratic fits are linear and optimal in a sense that they reduce the noise in parameters. However, you might interested in the reducing noise in the data instead. You might also want to ignore outliers since they can bia s your solutions greatly. Both problems can be solved with RANSAC. See my post at: