Plotting speed and distance calculated using accelerometer - matlab

I am working on basic distance calculation using accelerometer by dragging object on a single axis for physics class in MATLAB and have problem with plotting data.
My steps are:
1) After calibrating device to read zero g on every axis, I eliminate drift errors:
X_real = X_sample - X_calibrated;
if(X_real <= X_drift )
{
X_real = 0;
}
Where X_drift is 2 mg (From datasheet of accelerometer)
2) Calculate velocity:
velocity = 0; % On start
% Integration
v(i) = v(i-1) - x(i-1)-(x(i)+x(i-1)+x(i-2)+x(i-3))/4;
%Check if we stopped
if(x(i-1)==0 && x(i)==0)
v(i)=0;
end
%Check if velocity is under 0 (Not allowed)
if(v(i) < 0)
v(i)=0;
end
velocity = velocity + v(i);
3) Calculate distance:
distance = 0; % On start
%Integration
s(i) = s(i-1) + v(i-1) + (v(i)-v(i-1)-v(i-2)-v(i-3))/4;
distance = distance + s(i);
After testing this by dragging accelerometer on table 20 cm i got these results:
velocity = 0.09 m/s
distance = 0.21 m
time = 3.2s
Error of 1 cm is OK for classroom.
Chart tells something different:
I tried to plot distance after this:
s(i) = s(i)+s(i-1);
And got 21 cm on chart but after 6 s not after 4 s where it should be.
What am I doing wrong?
*UPDATE: Position y value is in mm not cm! Sorry

Im sorry for asking for help, i thought my formulas were ok, but they didn't. After step by step calculations my final solution is:
1) Velocity:
v(i) = v(i-1) - x(i-1)-(x(i)+x(i-1))/2;
2) Distance:
s(i) = s(i-1) + v(i-1)+(v(i)+v(i-1))/2;
And chart is:
Sorry once more time. I hope this will help someone calculating velocity and distance. It surely helped me as lesson to better study my code next time before asking for help.

Related

Manually building Hexagonal Torus

I am interested in building a hexagonal Torus using a mesh of points?
I think I can start with a 2-d polygon, and then iterate 360 times (1 deg resolution) to build a complete solid.
Is this the best way to do this? What I'm really after is building wing profiles with variable cross section geometry over it's span.
In Your way You can do this with polyhedron(). Add an appropriate number of points per profile in defined order to a vector „points“, define faces by the indices of the points in a second vector „faces“ and set both vectors as parameter in polyhedron(), see documentation. You can control the quality of the surface by the number of points per profile and the distance between the profiles (sectors in torus).
Here an example code:
// parameter:
r1 = 20; // radius of torus
r2 = 4; // radius of polygon/ thickness of torus
s = 360; // sections per 360 deg
p = 6; // points on polygon
a = 30; // angle of the first point on Polygon
// points on cross-section
// angle = 360*i/p + startangle, x = r2*cos(angle), y = 0, z = r2*sin(angle)
function cs_point(i) = [r1 + r2*cos(360*i/p + a), 0, r2*sin(360*i/p + a)];
// returns to the index in the points - vector the section number and the number of the point on this section
function point_index(i) = [floor(i/p), i - p*floor(i/p)];
// returns the points x-, y-, z-coordinates by rotatating the corresponding point from crossection around the z-axis
function iterate_cs(i) = [cs[point_index(i)[1]][0]*cos(360*floor(i/p)/s), cs[point_index(i)[1]][0]*sin(360*floor(i/p)/s), cs[point_index(i)[1]][2]];
// for every point find neighbour points to build faces, ( + p: point on the next cross-section), points ordered clockwise
// to connect point on last section to corresponding points on first section
function item_add1(i) = i >= (s - 1)*p ? -(s)*p : 0;
// to connect last point on section to first points on the same and the next section
function item_add2(i) = i - p*floor(i/p) >= p-1 ? -p : 0;
// build faces
function find_neighbours1(i) = [i, i + 1 + item_add2(i), i + 1 + item_add2(i) + p + item_add1(i)];
function find_neighbours2(i) = [i, i + 1 + + item_add2(i) + p + item_add1(i), i + p + item_add1(i)];
cs = [for (i = [0:p-1]) cs_point(i)];
points = [for (i = [0:s*p - 1]) iterate_cs(i)];
faces1 = [for (i = [0:s*p - 1]) find_neighbours1(i)];
faces2 = [for (i = [0:s*p - 1]) find_neighbours2(i)];
faces = concat(faces1, faces2);
polyhedron(points = points, faces = faces);
here the result:
Since openscad 2015-03 faces can have more than 3 points, if all points of the face are on the same plane. So in this case faces could be build in one step too.
Are you building smth. like NACA airfoils? https://en.wikipedia.org/wiki/NACA_airfoil
There are a few OpenSCAD designs for those floating around, see e.g. https://www.thingiverse.com/thing:898554

Find Position based on signal strength (intersection area between circles)

I'm trying to estimate a position based on signal strength received from 4 Wi-Fi Access Points. I measure the signal strength from 4 access points located in each corner of a square room with 100 square meters (10x10). I recorded the signal strengths in a known position (x, y) = (9.5, 1.5) using an Android phone. Now I want to check how accurate can a multilateration method be under the circumstances.
Using MATLAB, I applied a formula to calculate distance using the signal strength. The following MATLAB function shows the application of the formula:
function [ d_vect ] = distance( RSS )
% Calculate distance from signal strength
result = (27.55 - (20 * log10(2400)) + abs(RSS)) / 20;
d_vect = power(10, result);
end
The input RSS is a vector with the four signal strengths measured in the test point (x,y) = (9.5, 1.5). The RSS vector looks like this:
RSS =
-57.6000
-60.4000
-44.7000
-54.4000
and the resultant vector with all the estimated distances to each access points looks like this:
d_vect =
7.5386
10.4061
1.7072
5.2154
Now I want to estimate my position based on these distances and the access points position in order to find the error between the estimated position and the known position (9.5, 1.5). I want to find the intersection area (In order to estimate a position) between four circles where each access point is the center of one of the circles and the distance is the radius of the circle.
I want to find the grey area as shown in this image :
http://www.biologycorner.com/resources/venn4.gif
If you want an alternative way of estimating the location without estimating the intersection of circles you can use trilateration. It is a common technique in navigation (e.g. GPS) to estimate a position given a set of distance measurements.
Also, if you wanted the area because you also need an estimate of the uncertainty of the position I would recommend solving the trilateration problem using least squares which will easily give you an estimate of the parameters involved and an error propagation to yield an uncertainty of the location.
I found an answear that solved perfectly the question. It is explained in detail in this link:
https://gis.stackexchange.com/questions/40660/trilateration-algorithm-for-n-amount-of-points
I also developed some MATLAB code for the problem. Here it goes:
Estimate distances from the Access Points:
function [ d_vect ] = distance( RSS )
result = (27.55 - (20 * log10(2400)) + abs(RSS)) / 20;
d_vect = power(10, result);
end
The trilateration function:
function [] = trilat( X, d, real1, real2 )
cla
circles(X(1), X(5), d(1), 'edgecolor', [0 0 0],'facecolor', 'none','linewidth',4); %AP1 - black
circles(X(2), X(6), d(2), 'edgecolor', [0 1 0],'facecolor', 'none','linewidth',4); %AP2 - green
circles(X(3), X(7), d(3), 'edgecolor', [0 1 1],'facecolor', 'none','linewidth',4); %AP3 - cyan
circles(X(4), X(8), d(4), 'edgecolor', [1 1 0],'facecolor', 'none','linewidth',4); %AP4 - yellow
axis([0 10 0 10])
hold on
tbl = table(X, d);
d = d.^2;
weights = d.^(-1);
weights = transpose(weights);
beta0 = [5, 5];
modelfun = #(b,X)(abs(b(1)-X(:,1)).^2+abs(b(2)-X(:,2)).^2).^(1/2);
mdl = fitnlm(tbl,modelfun,beta0, 'Weights', weights);
b = mdl.Coefficients{1:2,{'Estimate'}}
scatter(b(1), b(2), 70, [0 0 1], 'filled')
scatter(real1, real2, 70, [1 0 0], 'filled')
hold off
end
Where,
X: matrix with APs coordinates
d: distance estimation vector
real1: real position x
real2: real position y
If you have three sets of measurements with (x,y) coordinates of location and corresponding signal strength. such as:
m1 = (x1,y1,s1)
m2 = (x2,y2,s2)
m3 = (x3,y3,s3)
Then you can calculate distances between each of the point locations:
d12 = Sqrt((x1 - x2)^2 + (y1 - y2)^2)
d13 = Sqrt((x1 - x3)^2 + (y1 - y3)^2)
d23 = Sqrt((x2 - x3)^2 + (y2 - y3)^2)
Now consider that each signal strength measurement signifies an emitter for that signal, that comes from a location somewhere at a distance. That distance would be a radius from the location where the signal strength was measured, because one would not know at this point the direction from where the signal came from. Also, the weaker the signal... the larger the radius. In other words, the signal strength measurement would be inversely proportional to the radius. The smaller the signal strength the larger the radius, and vice versa. So, calculate the proportional, although not yet accurate, radius's of our three points:
r1 = 1/s1
r2 = 1/s2
r3 = 1/s3
So now, at each point pair, set apart by their distance we can calculate a constant (C) where the radius's from each location will just touch one another. For example, for the point pair 1 & 2:
Ca * r1 + Ca * r2 = d12
... solving for the constant Ca:
Ca = d12 / (r1 + r2)
... and we can do this for the other two pairs, as well.
Cb = d13 / (r1 + r3)
Cc = d23 / (r2 + r3)
All right... select the largest C constant, either Ca, Cb, or Cc. Then, use the parametric equation for a circle to find where the coordinates meet. I will explain.
The parametric equation for a circle is:
x = radius * Cos(theta)
y = radius * Sin(theta)
If Ca was the largest constant found, then you would compare points 1 & 2, such as:
Ca * r1 * Cos(theta1) == Ca * r2 * Cos(theta2) &&
Ca * r1 * Sin(theta1) == Ca * r2 * Sin(theta2)
... iterating theta1 and theta2 from 0 to 360 degrees, for both circles. You might write code like:
for theta1 in 0 ..< 360 {
for theta2 in 0 ..< 360 {
if( abs(Ca*r1*cos(theta1) - Ca*r2*cos(theta2)) < 0.01 && abs(Ca*r1*sin(theta1) - Ca*r2*sin(theta2)) < 0.01 ) {
print("point is: (", Ca*r1*cos(theta1), Ca*r1*sin(theta1),")")
}
}
}
Depending on what your tolerance was for a match, you wouldn't have to do too many iterations around the circumferences of each signal radius to determine an estimate for the location of the signal source.
So basically you need to intersect 4 circles. There can be many approaches to it, and there are two that will generate the exact intersection area.
First approach is to start with one circle, intersect it with the second circle, then intersect the resulting area with the third circle and so on. that is, on each step you know current intersection area, and you intersect it with a new circle. The intersection area will always be a region bounded by circle arcs, so to intersect it with a new circle you walk along the boundary of the area and check whether each bounding arc intersects with a new circle. If it does, then you leave only the part of the arc that lies inside a new circle, remember that you should continue with an arc from a new circle, and continue traversing the boundary until you find the next intersection.
Another approach that seems to result in a worse time complexity, but in your case of 4 circles this will not be important, is to find all the intersection points of two circles and choose only those points that are of interest for you, that is which lie inside all other circles. These points will be the corners of your area, and then it is rather easy to reconstruct the area. After googling a bit, I have even found a live demo of this approach.

Matlabs ellipse hours calc

I make sundial simulator and i draw ellipse and then i have to draw hours on this ellipse. Every our is specified by:
x = a * sin(t);
y = b * cos(t);
where:
a- length of longer semi-axis
b- length of smaller semi-axis
t- hour in degrees ( 1 hour == 15 degrees)
I wrote this function in Matlab:
function [hx,hy] = calcHourCoords(ra,rb)
%input:
%ra, rb length of semi-axis in ellipse
%output:
%hx, hy coords of hour's plot
hourAngle = 15*180/pi;
step = 0;
for i=1:1:24
hx(i)= ra * sin(step);
hy(i)= rb * cos(step);
step = step+hourAngle;
end
end
Finally i get that pic:
My ellipse and hours points
But is should looks like:
Correct hour place's
Ellipse is correct ( I draw my version for other latitude ).
Maybe someone could help me ?
Sorry for my english :)
EDIT
I repair it - just convert degrees to radians.
EDIT2
I change source code FYI
function [hx,hy] = calcHourCoords(ra,rb)
%input:
%ra, rb length of semi-axis in ellipse
%output:
%hx, hy coords of hour's plot
hourAngle = 15*pi/180;
step = 0;
for i=1:1:24
hx(i)= ra * sin(step);
hy(i)= rb * cos(step);
step = step+hourAngle;
end
end

Point tangent to circle

Using this as a [reference][1]: Find a tangent point on circle?
cx = 0;
cy = 0;
px = -3;
py = -8;
dx = cx - px;
dy = cy - py;
a = asin(5 / ((dx*dx + dy*dy)^0.5));
b = atan2(dy, dx);
t_1 = deg2rad(180) + b - a;
t_2 = deg2rad(180) + b + a;
For a point (7,6) the angles are 7.9572/73.4434 and for (-3, -8) are 213.6264/285.2615. So for the first quadrant, the angles do not make sense, but for third quadrant they do. What I am doing wrong?
Your formula for a is wrong. You should use
a = acos(5 / ((dx*dx + dy*dy)^0.5))
instead of
a = asin(5 / ((dx*dx + dy*dy)^0.5))
i.e. use acos(...) instead of asin(...). The reason is shown in the image below. The formula for angle a is a=acos(r/H), where r is the radius of the circle and H is the length of the hypotenuse of the right angle triangle. So this has nothing to do with the fact that asin(...) has no way to know which of the two possible quadrants the value that is passed in lies. the argument of the asin is always positive, and you always want the answer in the range 0 to 90 degrees.
So the answer for the two angles that you want are b+a and b-a. Using acos instead of asin in your two cases produces 97.7592 & -16.5566 (or equivalently 343.4434) for your first quadrant example, and -164.7385 & -56.3736 (or equivalently 195.2615 and 303.6264) for your third quadrant example. (NB: instead of adding 180 degrees in the formula for t_1 and t-2, you could just switch the signs of dx and dy)
First -- I spent like 10 minutes figuring out what the heck you're trying to do (which in the end, I got from a comment in one of the answers), while solving your problem took 2 minutes. So, for future reference, please give a description of your problem as clear as you can first.
Now, I think you just have your signs messed up. Try the following:
%// difference vector
%// NOTE: these go the other way around for the atan2 to come out right
dx = px - cx;
dy = py - cy;
%// tip angle of the right triangle
a = asin( 5 / sqrt(dx*dx + dy*dy) );
%// angle between the (local) X-axis and the line of interest
b = atan2(dy, dx);
%// the third angle in the right triangle
%// NOTE: minus a here instead of plus b
g = pi/2 - a;
%// Angles of interest
%// NOTE1: signs are flipped; this automatically takes care of overshoots
%// NOTE2: don't forget to mod 360
t_1 = mod( rad2deg(b - g), 360)
t_2 = mod( rad2deg(b + g), 360)
Alternatively, you could skip computing the intermediate angle a by using acos instead of asin:
%// difference vector
dx = px - cx;
dy = py - cy;
%// Directly compute the third angle of the right triangle
%// (that is, the angle "at the origin")
g = acos( 5 / sqrt(dx*dx + dy*dy) );
%// angle between the (local) X-axis and the line of interest
b = atan2(dy, dx);
%// Angles of interest
t_1 = mod( rad2deg(b - g), 360)
t_2 = mod( rad2deg(b + g), 360)
Just another wayto re-discover the trigonometric identity acos(x) = pi/2 - asin(x) :)
This MathWorld entry is what you want: http://mathworld.wolfram.com/CircleTangentLine.html.
Alright, it looks like you are not accounting for the fact that asin, atan, ( any a-trig function ) has no way to know which of the two possible quadrants the value you passed in lies. To make up for that, a-trig function will assume that your point is in the first or fourth quadrant ( northeast / southeast ). Therefore, if you call atan function and your original point was in the second or third quadrant, you need to add 180 degrees / pi radians onto whatever value it returns.
See the documentation here stating that asin returns a value from [-pi/2, pi/2] :
http://www.mathworks.com/help/matlab/ref/asin.html
Hope that helps :)
EDIT
I misunderstood the situation originally
Here is what I think you have calculated :
t_1 and t_2 represent the angles you would travel at if you started on the circle from the tangent point and wanted to travel to your original starting point.
Viewed with this perspective your angles are correct.
For the point (7,6)
If you started on the circle at approx. (0,5) and traveled at 7 degrees, you would hit the point.
If you started on the circle at approx. (5,0) and traveled at 70 degrees, you would hit the point.
Now, what is going to be more useful and less confusing than angles, will be to know the slope of the line. To get this from the angle, do the following with angle in degrees:
angle = (angle + 90 + 360) % 180 - 90 // this gives us the angle as it would be in quad 1 or 4
slope = tan( deg2rad( angle ) )

Projectile Motion in Cocos2d iphone

I want to throw a ball that has a projectile motion. I have a monkey on centre of screen and onTouchBegin I am taking the starting point of the touch and onTouchEnded I am taking the ending points. From the starting and ending points I am taking the angle value between them. Like 30 degrees, 45 or 90 degree.
This is my code by which I have calculated angle of start to endpoint
float angleRadians = atan2(startTouchPoint.x - touchPoint.x, startTouchPoint.y - touchPoint.y);
float angleDegrees = CC_RADIANS_TO_DEGREES(angleRadians);
float cocosAngle = -1 * angleDegrees;
Now i am using Projectile motion formula to throw ball with angle i have calculated from above formula .
inside init method
gravity = 9.8; // metres per second square
X = 0;
Y = 0;
V0 = 50; // meters per second -- elevation
VX0 = V0 * cos(angle); // meters per second
VY0 = V0 * sin(angle); // meters per second
gameTime = 0;
and onTouchEnded i have called fire method which will throw ball .
-(void)fire:(ccTime) dt
{
CCLOG(#"Angle 1: %.2f",angle);
gameTime += dt*6;
// x = v0 * t * cos(angle)
X = (V0 * gameTime * cos(angle))/2+120;
// y = v0 * t * sin(angle) - 0.5 * g * t^2
Y = (V0 * gameTime * sin(angle) - 0.5 * gravity * pow(gameTime, 2))/2+255;
if (Y > 50)
{
sprite_webfire.position = ccp(X,Y);
flag = true;
}
else
{
//angleValue += 15;
angleValue = angle;
angle = [self DegreesToRadians:angleValue];
gravity = 9.8; // metres per second square
X = 0;
Y = 0;
V0 = 50; // meters per second -- elevation
VX0 = V0 * cos(angle); // meters per second
VY0 = V0 * sin(angle); // meters per second
gameTime = 0;
// [self pauseSchedulerAndActions];
}
if (Y < 50)
{
[self unschedule:#selector(fire:)];
}
NSLog(#"ball (%lf,%lf), dt = %lf angle value %d", X, Y, dt,angleValue);
}
this code is working . by this code i can throw ball in projectile motion but i cant throw it where i want to. i cant throw wrt to given angle from start to end point.
i can throw it like red mark but i want to throw it blue mark with swipe . but its not throwing like i am swiping screen.
I am not certain on what math you are using to do this, I find your documentation a bit confusing.
Generally, for project tile motion this is what you need to do:
Find out what the take off angle is relative to the horizontal. Then depending on whatever initial velocity you want the object to have, use that and you trig equations to put your initial velocities into rectangular components.
For example:
If initial velocity was 10, the initial velocity in the y direction would be 10sin(angle), and in the x direction it would be 10cos(angle).
Then in to update the position of the sprite you should use kinematics equations: http://www.physicsclassroom.com/class/1dkin/u1l6c.cfm
First update velocities:
Velocity in the Y direction: V = v(initial) + gravity*(Delta-time)
Velocity in the X direction is constant unless you want to factor in some sort of resistance to make things a lot more complicated.
then position y = oldPositionY + velocity(in Y direction)*(Delta-time) + 1/2(gravity)(delta-time)^2.
and position x = oldPositionX + Xvelocity*delta-time
I have done some projectile motion stuff, and I have found you need to make gravity a large constant, something around 500 to make it look life-like. Let me know if this is confusing or you don't know how to implement it.
I would suggest that you take a look at the following tutorial: http://www.raywenderlich.com/4756/how-to-make-a-catapult-shooting-game-with-cocos2d-and-box2d-part-1.
It shows you how to use a physics engine, so you don't need to do much of the math. All the 'bullets' in the tutorial are also moving with projectile motion.
I'll add a bit to what was already said (which was good). Firstly, you should not be wasting time computing any angles. Stick with vectors for your velocity. In other words, get the initial velocity vector from the touch start and end location, and that will be your (v0x, v0y). For example:
CGPoint initialVelocity = ccpSub(touchPoint, startTouchPoint);
float v0x = initialVelocity.x;
float v0y = initialVelocity.y;
If you wish to assign a different magnitude to the initial velocity vector, simply normalize it and then multiply it by a new magnitude.
CGPoint unitVelocity = ccpNormalize(initialVelocity);
float magnitude = 200; // or whatever you want it to be
CGPoint velocity = ccpMult(unitVelocity, magnitude);
Anyway, with this velocity set properly you can then use it in your position calculations as before, but without the added complexity of calculating the angles.
-(void) fire:(ccTime)dt
{
.
.
gameTime += dt;
// if x(t) = x0 + v0x*t, then dx = v0x*dt
x += v0x*dt;
// if y(t) = y0 + v0y*t - 0.5t^2, then dy = v0y*dt - g*t*dt
y += (v0y * dt - g*gameTime*dt);
.
.
}
Also you should not set v0 = 50. Calculate the velocity from the vector as I suggested.
Something important to consider is that you are calculating what the movement should be in a physical world based upon units of meters. The screen is operating in points, not meters, so you will probably have to apply a scaling factor to the new position (x,y) to get the look that you are going for.
Edit: my bad, I had to revisit my math in the position calculation. My differentials was a bit rusty.