I am using extended kalman filter to fuse accelerometer, gyro, and magnetometer data. I use accelerometer to correct pitch and roll data, and magnetometer to correct yaw. The pitch and roll are working well, but i have a very severe yaw drifting even though I implemented the magnetometer. The code I'm using to fuse magnetometer data in the EKF is:
(m being the magnetometer measurements and a being the accelerometer measurements)
m_max.x = +540; m_max.y = +500; m_max.z = 180;
m_min.x = -520; m_min.y = -570; m_min.z = -770;
m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
vector temp_a = a;
// normalize
vector_normalize(&temp_a);
//vector_normalize(&m);
// compute E and N
vector E;
vector N;
vector_cross(&m,&temp_a,&E);
vector_normalize(&E);
vector_cross(&temp_a,&E,&N);
// q is the state quaternion matrix
Xog = [1-2(q2*q2+q3*q3);
2(q1*q2+q0*q3)];
Xogmag = [N;E];
// yaw error
Ey = Xogmag - Xog;
// yaw observation matrix
Hy = [0, 0, -4*q2, -4*q3, 0, 0, 0;
w*q3, 2*q2, 2*q1, 2*q0, 0, 0, 0];
// yaw estimation error covariance matrix
Py - Hy * P * (Hy') + Ry
// yaw kalman gain
Ky = P * (Hy') * inv(Py);
// update the state
X = X + Ky * Ey;
// update system state covariance matrix
P = P - Ky * Hy * P;
I'm not completely sure about how to fuse the magnetometer data. If you know what is wrong with the code or how I could fix it, please let me know!
Thanks a lot!
This is an overloaded question... to implement something like that you'd need to first understand at least:
(1) nuances of sensor noise and behavior on device, for example the magnetometer will typically break KF assumptions
(2) what is the state transition model, i.e. what is the relationship between changes in pitch/yaw and changes in magnetic field
Related
I am developing a finite element software that minimizes the energy of a mechanical structure. Using octave and its optim package, I run into a strange issue: The lm_feasible algorithm doesn't calculate at all when I use more than 300 degrees of freedom (DoF). Another algorithm (sqp) performs the calculation but doesn't work well when I complexify the structure and are out of my test case.
Is there a limit in the number of DoF with lm_feasible algorithm?
If so, how many DoF are maximally possible?
To give an overview and general idea of how the code works:
[x,y] = geometryGenerator()
U = zeros(lenght(x)*2,1);
U(1:2:end-1) = x;
U(2:2:end) = y;
%Non geometric argument are not optimised, and fixed during calculation
fct =#(U)complexFunctionOfEnergyIWrap(U(1:2:end-1),U(2:2:end), variousMaterialPropertiesAndOtherArgs)
para = optimset("f_equc_idx",contEq,"lb",lb,"ub",ub,"objf_grad",dEne,"objf_hessian",d2Ene,"MaxIter",1000);
[U,eneFinale,cvg,outp] = nonlin_min(fct,U,para)
Full example:
clear
pkg load optim
function [x,y] = geometryGenerator(r,elts = 100)
teta = linspace(0,pi,elts = 100);
x = r * cos(teta);
y = r * sin(teta);
endfunction
function ene = complexFunctionOfEnergyIWrap (x,y,E,P, X,Y)
ene = 0;
for i = 1:length(x)-1
ene += E*(x(i)/X(i))^4+ E*(y(i)/Y(i))^4- P *(x(i)^2+(x(i+1)^2)-x(i)*x(i+1))*abs(y(i)-y(i+1));
endfor
endfunction
[x,y] = geometryGenerator(5,100)
%Little distance from axis to avoid division by zero
x +=1e-6;
y +=1e-6;
%Saving initial geometry
X = x;
Y = y;
%Vectorisation of the function
%% Initial vector
U = zeros(length(x)*2,1);
U(1:2:end-1) = linspace(min(x),max(x),length(x));
U(2:2:end) = linspace(min(y),max(y),length(y));
%%Constraints
Aeq = zeros(3,length(U));
%%% Blocked bottom
Aeq(1,1) = 1;
Aeq(2,2) = 1;
%%% Sliding top
Aeq(3,end-1) = 1;
%%%Initial condition
beq = zeros(3,1);
beq(1) = U(1);
beq(2) = U(2);
beq(3) = U(end-1);
contEq = #(U) Aeq * U - beq;
%Parameter
Mat = 0.2e9;
pressure = 50;
%% Vectorized function. Non geometric argument are not optimised, and fixed during calculation
fct =#(U)complexFunctionOfEnergyIWrap(U(1:2:end-1),U(2:2:end), Mat, pressure, X, Y)
para = optimset("Algorithm","lm_feasible","f_equc_idx",contEq,"MaxIter",1000);
[U,eneFinale,cvg,outp] = nonlin_min(fct,U,para)
xFinal = U(1:2:end-1);
yFinal = U(2:2:end);
plot(x,y,';Initial geo;',xFinal,yFinal,'--x;Final geo;')
Finite Element Method is typically formulated as the optimal criteria for the minimization problem, which is equivalent to the Virtual Work Principle (see books like Hughes of Bathe). The Virtual Work, represents a set of linear (or nonlinear) equations which can be solved more efficiently (with fsolve).
If for some motive you must solve the problem as an optimization problem, then, if you are considering linear elasticity, your strain energy is quadratic, thus you could use the qp Octave function.
To use sparse matrices could also be helpful.
I need help in understanding the Caffe function, SigmoidCrossEntropyLossLayer, which is the cross-entropy error with logistic activation.
Basically, the cross-entropy error for a single example with N independent targets is denoted as:
- sum-over-N( t[i] * log(x[i]) + (1 - t[i]) * log(1 - x[i] )
where t is the target, 0 or 1, and x is the output, indexed by i. x, of course goes through a logistic activation.
An algebraic trick for quicker cross-entropy calculation reduces the computation to:
-t[i] * x[i] + log(1 + exp(x[i]))
and you can verify that from Section 3 here.
The question is, how is the above translated to the loss calculating code below:
loss -= input_data[i] * (target[i] - (input_data[i] >= 0)) -
log(1 + exp(input_data[i] - 2 * input_data[i] * (input_data[i] >= 0)));
Thank you.
The function is reproduced below for convenience.
template <typename Dtype>
void SigmoidCrossEntropyLossLayer<Dtype>::Forward_cpu(
const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top) {
// The forward pass computes the sigmoid outputs.
sigmoid_bottom_vec_[0] = bottom[0];
sigmoid_layer_->Forward(sigmoid_bottom_vec_, sigmoid_top_vec_);
// Compute the loss (negative log likelihood)
// Stable version of loss computation from input data
const Dtype* input_data = bottom[0]->cpu_data();
const Dtype* target = bottom[1]->cpu_data();
int valid_count = 0;
Dtype loss = 0;
for (int i = 0; i < bottom[0]->count(); ++i) {
const int target_value = static_cast<int>(target[i]);
if (has_ignore_label_ && target_value == ignore_label_) {
continue;
}
loss -= input_data[i] * (target[i] - (input_data[i] >= 0)) -
log(1 + exp(input_data[i] - 2 * input_data[i] * (input_data[i] >= 0)));
++valid_count;
}
normalizer_ = get_normalizer(normalization_, valid_count);
top[0]->mutable_cpu_data()[0] = loss / normalizer_;
}
In the expression log(1 + exp(x[i])) you might encounter numerical instability in case x[i] is very large. To overcome this numerical instability, one scales the sigmoid function like this:
sig(x) = exp(x)/(1+exp(x))
= [exp(x)*exp(-x(x>=0))]/[(1+exp(x))*exp(-x(x>=0))]
Now, if you plug the new and stable expression for sig(x) into the loss you'll end up with the same expression as caffe is using.
Enjoy!
I'm using AS3 to program some collision detection for a flash game and am having trouble figuring out how to bounce a ball off of a line. I keep track of a vector that represents the ball's 2D velocity and I'm trying to reflect it over the vector that is perpendicular to the line that the ball's colliding with (aka the normal). My problem is that I don't know how to figure out the new vector (that's reflected over the normal). I figured that you can use Math.atan2 to find the difference between the normal and the ball's vector but I'm not sure how to expand that to solve my problem.
Vector algebra - You want the "bounce" vector:
vec1 is the ball's motion vector and vec2 is the surface/line vector:
// 1. Find the dot product of vec1 and vec2
// Note: dx and dy are vx and vy divided over the length of the vector (magnitude)
var dpA:Number = vec1.vx * vec2.dx + vec1.vy * vec2.dy;
// 2. Project vec1 over vec2
var prA_vx:Number = dpA * vec2.dx;
var prA_vy:Number = dpA * vec2.dy;
// 3. Find the dot product of vec1 and vec2's normal
// (left or right normal depending on line's direction, let's say left)
var dpB:Number = vec1.vx * vec2.leftNormal.dx + vec1.vy * vec2.leftNormal.dy;
// 4. Project vec1 over vec2's left normal
var prB_vx:Number = dpB * vec2.leftNormal.dx;
var prB_vy:Number = dpB * vec2.leftNormal.dy;
// 5. Add the first projection prA to the reverse of the second -prB
var new_vx:Number = prA_vx - prB_vx;
var new_vy:Number = prA_vy - prB_vy;
Assign those velocities to your ball's motion vector and let it bounce.
PS:
vec.leftNormal --> vx = vec.vy; vy = -vec.vx;
vec.rightNormal --> vx = -vec.vy; vy = vec.vx;
The mirror reflection of any vector v from a line/(hyper-)surface with normal n in any dimension can be computed using projection tensors. The parallel projection of v on n is: v|| = (v . n) n = v . nn. Here nn is the outer (or tensor) product of the normal with itself. In Cartesian coordinates it is a matrix with elements: nn[i,j] = n[i]*n[j]. The perpendicular projection is just the difference between the original vector and its parallel projection: v - v||. When the vector is reflected, its parallel projection is reversed while the perpendicular projection is retained. So the reflected vector is:
v' = -v|| + (v - v||) = v - 2 v|| = v . (I - 2 nn) = v . R( n ), where
R( n ) = I - 2 nn
(I is the identity tensor which in Cartesian coordinates is simply the diagonal identity matrix diag(1))
R is called the reflection tensor. In Cartesian coordinates it is a real symmetric matrix with components R[i,j] = delta[i,j] - 2*n[i]*n[j], where delta[i,j] = 1 if i == j and 0 otherwise. It is also symmetric with respect to n:
R( -n ) = I - 2(-n)(-n) = I - 2 nn = R( n )
Hence it doesn't matter if one uses the outward facing or the inward facing normal n - the result would be the same.
In two dimensions and Cartesian coordinates, R (the matrix representation of R) becomes:
[ R00 R01 ] [ 1.0-2.0*n.x*n.x -2.0*n.x*n.y ]
R = [ ] = [ ]
[ R10 R11 ] [ -2.0*n.x*n.y 1.0-2.0*n.y*n.y ]
The components of the reflected vector are then computed as a row-vector-matrix product:
v1.x = v.x*R00 + v.y*R10
v1.y = v.x*R01 + v.y*R11
or after expansion:
k = 2.0*(v.x*n.x + v.y*n.y)
v1.x = v.x - k*n.x
v1.y = v.y - k*n.y
In three dimensions:
k = 2.0*(v.x*n.x + v.y*n.y + v.z*n.z)
v1.x = v.x - k*n.x
v1.y = v.y - k*n.y
v1.z = v.z - k*n.z
Finding the exact point where the ball will hit the line/wall is more involved - see here.
Calculate two components of the vector.
One component will be the projection of your vector onto the reflecting surface the other component will be the projection on to the surface's normal (which you say you already have). Use dot products to get the projections. Add these two components together by summing the two vectors. You'll have your answer.
You can even calculate the second component A2 as being the original vector minus the first component, so: A2 = A - A1. And then the vector you want is A1 plus the reflected A2 (which is simply -A2 since its perpendicular to your surface) or:
Ar = A1-A2
or
Ar = 2A1 - A which is the same as Ar = -(2A2 - A)
If [Ax,Bx] is your balls velocity and [Wx,Wy] is a unit vector representing the wall:
A1x = (Ax*Wx+Ay*Wy)*Wx;
A1y = (Ax*Wx+Ay*Wy)*Wy;
Arx = 2*A1x - Ax;
Ary = 2*A1y - Ay;
I'm trying to get the skew values out of a transformation matrix in a flash movie clip. The transformation matrix is represented by
a b tx
c d ty
0 0 1
I have no information on what kind of transformation is performed and which comes first. I do know that in flash, you may only rotate OR skew a movie clip (correct me if I am wrong). I can get scale values from scaleX and scaleY properties of the movie clip. I believe translation does not quite matter i can just equate tx and ty to zero.
so my question has 2 parts. How do I determine if a skew or a rotation had been applied, and how do I get the respective values?
The 2D rotation matrix is
cos(theta) -sin(theta)
sin(theta) cos(theta)
so if you have no scaling or shear applied,
a = d
and
c = -b
and the angle of rotation is
theta = asin(c) = acos(a)
If you've got scaling applied and can recover the scaling factors sx and sy, just divide the first row by sx and the second by sy in your original transformation matrix and then recover the rotation angle as above.
If you've got a shear (skew) applied anywhere in there, I'm with the previous commenters, it might not be possible except in very limited cases (such as shear in only one known direction at a time and in a known order).
You need to do a polar decomposition. This Wikipedia article explains how it works:
http://en.wikipedia.org/wiki/Polar_decomposition
Here is the code I wrote for my own program using the OpenCV library.
const double PI = 3.141592653;
cv::Mat rotationOutput = cv::Mat::zeros(warp00.size(),CV_64F);
cv::Mat_<double>::iterator rotIter = rotationOutput.begin<double>();
cv::Mat_<double>::iterator warp00Iter = warp00.begin<double>();
cv::Mat_<double>::iterator warp01Iter = warp01.begin<double>();
cv::Mat_<double>::iterator warp10Iter = warp10.begin<double>();
cv::Mat_<double>::iterator warp11Iter = warp11.begin<double>();
for(; warp00Iter != warp00.end<double>(); ++warp00Iter, ++warp01Iter, ++warp10Iter,
++warp11Iter, ++rotIter){
cv::Matx22d fMatrix(*warp00Iter,*warp01Iter, *warp10Iter, *warp11Iter);
cv::Matx22d cMatrix;
cv::Matx22d cMatSqrt(0.,0.,0.,0.);
cv::mulTransposed(fMatrix, cMatrix, true);
cv::Matx21d eigenVals;
cv::Matx22d eigenVecs;
if((cMatrix(0,0) !=0.) && (cMatrix(1,1) !=0.)){
if(cv::eigen(cMatrix,true,eigenVals,eigenVecs)){
cMatSqrt = eigenVecs.t()*
cv::Matx22d(sqrt(eigenVals(0,0)),0.,0.,sqrt(eigenVals(1,0)))*eigenVecs;
}
}
cv::Matx22d rMat = fMatrix*cMatSqrt.inv();
*rotIter = atan(rMat(1,0)/rMat(0,0));
}
warp00, warp01, warp10 and warp11 contains the first 4 params of the affine transform (translation params warp02 and warp12 are not needed). IN your case it would be a,b,c,d.
You'll notice in the wikipedia article that you need to compute the square root of a matrix. The only way to do so is by computing the eigen values, then compute their square roots and rotate the diagonal matrix back to the original coordinate system.
It's complicated, but it is the only way to compute the rotations when you have an affine transform.
In my case, I only cared about the rotations, so my code won't give you the skew.
The term for this is matrix decomposition. Here is a solution that includes skew as described by Frédéric Wang.
Works when transforms are applied in this order: skew, scale, rotate, translate.
function decompose_2d_matrix(mat) {
var a = mat[0];
var b = mat[1];
var c = mat[2];
var d = mat[3];
var e = mat[4];
var f = mat[5];
var delta = a * d - b * c;
let result = {
translation: [e, f],
rotation: 0,
scale: [0, 0],
skew: [0, 0],
};
// Apply the QR-like decomposition.
if (a != 0 || b != 0) {
var r = Math.sqrt(a * a + b * b);
result.rotation = b > 0 ? Math.acos(a / r) : -Math.acos(a / r);
result.scale = [r, delta / r];
result.skew = [Math.atan((a * c + b * d) / (r * r)), 0];
} else if (c != 0 || d != 0) {
var s = Math.sqrt(c * c + d * d);
result.rotation =
Math.PI / 2 - (d > 0 ? Math.acos(-c / s) : -Math.acos(c / s));
result.scale = [delta / s, s];
result.skew = [0, Math.atan((a * c + b * d) / (s * s))];
} else {
// a = b = c = d = 0
}
return result;
}
First, you can do both skew and rotate, but you have to select the order first. A skew matrix is explained here, to add a skew matrix to a transformation you create a new matrix and do yourTransformMatrix.concat(skewMatrix);
I can't currently say if you can retrieve values for transformation in terms of "rotation angle", "skew_X angle", "skew_Y angle", "translation_X","translation_Y", this in general is a nonlinear equation system which might not have a solution for a specific matrix.
When given 0,0 to 0,5, the y velocity becomes that number and breaks my code. I know I must have done something wrong as I just copy and pasted code (since I am horrible at maths)..
This is how I calculate the numbers:
var radian = Math.atan2(listOfNodes[j].y - listOfNodes[i].y,listOfNodes[j].x - listOfNodes[i].x);
var vy = Math.cos(radian);
var vx = Math.sin(radian);
Thanks
There i am assuming the velocity vector is FROM 0,0 TO 0,5. And 0,0 is i and 0,5 is j.
In that case the velocity vector is only along y and the y component should be 5 and x component 0. It is coming as opposite because,
cos(radian) whould be x velocity component and sin(radian) the y compunent.
And the number 6.123031769111886E-17 is actually returned in place of 0.
Look at the following figure:
Also as can be seen from the figure you do not need the trigonometric computations at all.
You can simply get the x and y components as follows:
// y2 - y1
var vy = listOfNodes[j].y - listOfNodes[i].y;
// x2 - x1
var vx = listOfNodes[j].x - listOfNodes[i].x;
This will avoid the floating point inaccuracy caused by the trig finctions due to which you are seeing 6.123031769111886E-17 instead of 0.
You only need to use atan2 if you actually need the angle θ in your code.
Update:
Well if you need only unit (normalized) vector's components you can divide the vx and vy with the length of the original vector. Like this:
// y2 - y1
var vy = listOfNodes[j].y - listOfNodes[i].y;
// x2 - x1
var vx = listOfNodes[j].x - listOfNodes[i].x;
// vector magnitude
var mag = Math.sqrt(vx * vx + vy * vy);
// get unit vector components
vy /= mag;
vx /= mag;
Using the above you will get the exactly the same results as you are getting from trig sin and cos functions.
But if you still need to use the original code and want to make 6.12...E-17 compare to 0, you can use the epsilon technique for comparing floats. So you can compare any value within epsilon's range from 0, using flllowing code:
function floatCompare(a:Number, b:Number, epsilon:Number):Boolean{
return (a >= (b - epsilon) && a <= (b + epsilon));
}
// To check for zero use this code, here i'm using 0.0001 as epsilon
if(floatCompare(vx, 0, 0.0001)){
// code here
}
So any deviation in the range of [b-epsilon, b+epsilon] would successfully compare to b. This is essential in case of floating point arithmetic.