How do I calculate tilt-compensated yaw? - java

I've been struggling for some time now on how to correctly calculate the yaw angle from an IMU, but can't get it to work. I'm using the LSM9DS1, if that matters.
I already have proper values for roll and pitch. The value for yaw is also more or less correct until I start tilting the device. Therefore I have to implement some kind of tilt compensation.
I calculate the euler angles as follows:
double weight = 0.05f;
private void calculateEulerAngles() {
// Measured angle by the accelerometer
double rollXLMeasured = Math.atan2(this.accelerometer.getX(), this.accelerometer.getZ()) / 2 / Math.PI * 360;
double pitchXLMeasured = Math.atan2(this.accelerometer.getY() / 9.81f, this.accelerometer.getZ() / 9.81f) / 2 / Math.PI * 360;
// Adding a low pass filter
double rollXLFiltered = (1 - this.weight) * rollXLFilteredOld + this.weight * rollXLMeasured;
double pitchXLFiltered = (1 - this.weight) * pitchXLFilteredOld + this.weight * pitchXLMeasured;
this.rollXLFilteredOld = rollXLFiltered;
this.pitchXLFilteredOld = pitchXLFiltered;
// Calculating deltaTime
long time = System.nanoTime();
int difference = (int) ((time - this.deltaTime) / 1000000000);
this.deltaTime = time;
// Adding a complementary filter
this.roll = ((1 - this.weight) * (this.roll + this.gyroscope.getY() * difference)) + (this.weight * rollXLMeasured);
this.pitch = ((1 - this.weight) * (this.pitch - this.gyroscope.getX() * difference)) + (this.weight * pitchXLMeasured);
// Calculating yaw using the magnetometer and applying a low pass filter
double rollRadians = this.roll / 360 * (2 * Math.PI);
double pitchRadians = this.pitch / 360 * (2 * Math.PI);
double magnetometerXCompensated = (-this.magnetometer.getX() * Math.cos(rollRadians)) - (this.magnetometer.getY() * Math.sin(pitchRadians) *
Math.sin(rollRadians)) + (this.magnetometer.getZ() * Math.cos(pitchRadians) * Math.sin(rollRadians));
double magnetometerYCompensated = (this.magnetometer.getY() * Math.cos(pitchRadians)) + (this.magnetometer.getZ() * Math.sin(pitchRadians));
double yawMeasured = Math.atan2(magnetometerYCompensated, magnetometerXCompensated) / (2 * Math.PI) * 360;
double yawFiltered = (1 - this.weight) * yawFilteredOld + this.weight * yawMeasured;
this.yawFilteredOld = yawFiltered;
this.yaw = yawFiltered;
// Print roll, pitch and yaw for debug purposes
System.out.println(this.roll + ", " + this.pitch + ", " + this.yaw);
}
I don't include the whole code I use, since I think it's clear what the above code does and this is the part thats essential to the problem, I suppose.
And again, I get correct values, just not for yaw when I tilt the device. So there has to be an error concerning the math.
Do I have to do my calculations with the raw values, like the data thats inside the IMU, or use already processed data? For example this.accelerometer.getX() actually returns x_raw * 0.061f / 1000 * 9.81f with x_raw being the value stored inside the IMU and 0.061f being some coeffiecent. I basically copied the calculation from the Adafruit library, I'm not 100% sure why you have to multiply/divide those values though.
Also, you might have noticed that when I calculate the magnetometerXCompensated value I invert the x-axis. I do this, because the magnetometer axis aren't aligned with the acceleromter/gyroscope axis, so in order to align them I have to flip the x-axis.
Does anyone have an idea on how to solve this? I'm really tired of it not working properly, since I tried for quite a while now to solve it, but I'm not getting the results I wanted.

You can get all the equations from the next picture. The complimentary/Kalman filter is there in order to get less noise.

Related

Why sprite twitches?

Wrote a function that calculates the coordinates of a sprite that moves in a circle.
Well, the formula is elementary
x = x0 + R * cos (t * a + a0)
y = y0 + R * sin (t * a + a0)
The function looks like this
public void pointDefinition(float delta)
{
sprite.setPosition(Gdx.graphics.getWidth()/2+(85*(float)Math.cos(i*delta)), Gdx.graphics.getHeight()/2+(85*(float)Math.sin(i*delta)));
i = i + 0.5;
}
I call it in render ()
In the end, everything works, but the sprite somehow nervously twitches.
Here is a video that demonstrates this.
video
The following content is sent to the delta variable.
pointDefinition(Math.min(Gdx.graphics.getDeltaTime(), 1 / 60f));
I can not understand the reason
You should increment i with delta instead of using delta as a parameter in the formula:
public void pointDefinition(float delta)
{
sprite.setPosition((Gdx.graphics.getWidth() / 2) + (85 * (float)Math.cos(i)), (Gdx.graphics.getHeight() / 2) + (85 * (float)Math.sin(i)));
i = i + delta;
}
After all, delta will have different values each time, which is what is causing your little icon to move back sometimes.
To understand what is happening, imagine these two iterations:
First iteration
i = 2
delta = 0.0025
In this case, i * delta will be 0.005.
Second iteration
i = 2.5
delta = 0.0017
In this case, i * delta will be 0.00425.
As you can see, the value of i * delta, which you are using as the parameter of Math.sin() and Math.cos(), decreased in the second iteration, causing your icon to jitter.
To keep the animation consistent, it is important to make sure the parameter you pass to these two functions (Math.sin() and Math.cos()) only increases with each iteration!

Java Operations in Math Equations

I am trying to write a program that calculates the distance of a projectile, but the distance returned is not coming out correct. I am familiar with operator precedence in Java, but I am not sure why I am not getting the correct distance. For angle = 22, velocity = 35, and height = 10 I expect to get 75.54 but instead I am getting 42.03.
Are there obvious mistakes in my code that are causing this?
public class FootballDistanceCalculator {
public static final double GRAVITATIONAL_ACCELERATION = 32.174;
/**
* Calculates the distance a projectile travels
*
* #param angle angle at which projectile is thrown in degrees
* #param velocity initial velocity of projectile in miles/hour
* #param height initial height of projectile in feet
* #return distance traveled by projectile in feet
*/
public static double calculateDistance(double angle, double velocity, double height) {
double angleRadians = Math.toRadians(angle);
double vCosineThetaOverG = (velocity * (Math.cos(angleRadians))) / GRAVITATIONAL_ACCELERATION ;
double vSinTheta = velocity * (Math.sin(angleRadians));
double vSinThetaSquared = (Math.pow(vSinTheta, 2));
double twoGravHeight = (2 * GRAVITATIONAL_ACCELERATION * height);
double radical = Math.sqrt((vSinThetaSquared + twoGravHeight));
double distance = vCosineThetaOverG * (vSinTheta + radical);
return distance;
}
}
This is the equation I am basing this program off of:
d = (v cos(θ) / g)(v sin(θ) + √(v sin(θ)2 + 2 g h))
v = velocity
g = gravitational acceleration
h = height
The problem turned out to be a units conversion issue as indicated in the comments.
I had to take my velocity parameter and multiply by feet per mile (5280) and divide by seconds per hour (3600) to get my units to match.
You are calculating in metric but your g constant is in feet per second per second.

Directional helix for game missiles

my math isnt too great but im trying to learn though..
What im trying to do is give my games missiles a helix rocket effect, but i dont know how to work the Sin and Cos to make the helix play out in the right direction..
This is a 3D game by the way:
The problem is, depending on which direction the missile faces, the helix looks warped or flat..
Whats the best way to mathematically calculate a helix based on the missiles X,Y,Z/direction?, ive been trying to figure it out for a long time :/
Thanks alot!
double x = radius * Math.cos(theta);
double y = radius * Math.sin(theta);
double z = radius * Math.cos(theta);
location.add(x,y,z);
missile.shootFlame(location,2);
location.subtract(x,y,z);
Basis vectors
you need the overall direction of missile as 3D vector let call it W. From it you need to get 2 another vectors U,V which are all perpendicular to each other. To get them you can exploit cross product. So:
make W unit vector
Just W = W/|W| so:
l=sqrt(Wx*Wx+Wy*Wy+Wz*Wz);
Wx/=l;
Wy/=l;
Wz/=l;
choose U as any direction non parallel to W
so start with U=(1.0,0.0,0.0) and if U==W choose U=(0.0,1.0,0.0). If you got anything to lock to use that as U direction so the coordinate system will not rotate with time (like Up,North,Sun ...)
U should be unit so if not normalize it just like in #1
compute V
It should be perpendicular to U,W so use cross product:
V = W x U
Cross product of unit vectors is also unit vector so no need to normalize.
recompute U
As we choose the U manually we need to make sure it is also perpendicular to V,W so:
U = V x W
Now we have 3 perpendicular basis vectors where U,V lies in plane of the helix screws and W is the overall movement direction.
If you do not know how to compute the cross product see:
Understanding 4x4 homogenous transform matrices look for [edit2].
Now the Helix is easy:
Defines
so we have U,V,W on top of that we need radius r [units], movement speed v [units/s], angular speed o [rad/s] time t>=0.0 [s] and start position P0.
Helix equation
What we need is equation returning actual position for time so:
ang = o*t;
P(t) = P0 + U*r*cos(ang) + V*r*sin(ang) + W*v*t;
rewritten to scalars:
ang = o*t;
x = P0x + Ux*r*cos(ang) + Vx*r*sin(ang) + Wx*v*t;
y = P0y + Uy*r*cos(ang) + Vy*r*sin(ang) + Wy*v*t;
z = P0z + Uz*r*cos(ang) + Vz*r*sin(ang) + Wz*v*t;
[edit1] as you are incompetent to copy paste and or changing my code correctly...
Vector w = loc.getDirection();
double wX = w.getX();
double wY = w.getY();
double wZ = w.getZ();
double l = Math.sqrt((wX * wX) + (wY * wY) + (wZ * wZ));
wX = wX / l;
wY = wY / l;
wZ = wZ / l;
w = new Vector(wX,wY,wZ); // you forget to change W and use it latter ...
Vector u = new Vector(0, 1.0, 0);
if (Math.abs(wX)<1e-3) // if U and W are the same chose different U
if (Math.abs(wZ)<1e-3)
u = new Vector(1.0, 0.0, 0);
Vector v = w.crossProduct(u);
u = v.crossProduct(w);
double radius = 10; // helix radius [units]
double speed = 2.00; // movement speed [unit/s]
double omega = 0.628; // angular speed [rad/s]
//double omega = 36.0; // angular speed [deg/s]
for (double i = 0; i < 100; i += 1.0) // time [s]
{
double angle = omega * i; // actual screw position [rad] or [deg]
double x = u.getX() * radius * Math.cos(angle) + v.getX() * radius * Math.sin(angle) + wX * speed * i;
double y = u.getY() * radius * Math.cos(angle) + v.getY() * radius * Math.sin(angle) + wY * speed * i;
double z = u.getZ() * radius * Math.cos(angle) + v.getZ() * radius * Math.sin(angle) + wZ * speed * i;
loc.add(x,y,z); // What is this? should not you set the x,y,z instead of adding?
//do work
loc.subtract(x,y,z); // what is this?
}
This should provide you with helix points with traveled linear distance
speed*imax = 2.0*100.0 = 200.0 units
And screws:
omega*imax/(2*Pi) ~= 0.628*100.0/6.28 ~= 10 screws // in case of sin,cos want [rad]
omega*imax/360.0 = 36.0*100.0/360 = 10.0 screws // in case of sin,cos want [deg]
Do not forget to rem in/out the correct omega line (I choose [rad] as that is what I am used that my math libs use). Not sure If I translated to your environment correctly there may be bugs like abs has different name or u = new Vector(1.0, 0.0, 0); can be done on intermediate or declaration of variable only etc which I do not know as I do not code in it.

Char movement, getting x/y speeds from 2 points

So i'm trying to make a program in java and i need to figure out how much I should add to x and y to make the player movement a constant speed at any angle. The information i have is 2 points, a starting point and an and and I need the char to move from a to b at a constant speed. I'm currently trying to figure out a bunch of different Math methods but I just cant get it right. Thanks in advance.
It's actually quite simple. You have points A (A.x, A.y) and B (B.x, B.y) and need the update for your character position.
Start by calculating the direction vector dir = B - A (subtract component-wise, such that dir.x = B.x - A.x; dir.y = B.y - A.y).
If you add this entire vector to your character's position, you will move it by sqrt(dir.x^2 + dir.y^2) (Pythagorean theorem). Hence, the speed will be: speed = sqrt(dir.x^2 + dir.y^2) / frameTime.
So if you want a constant speed, you have to find a multiple of the direction vector. This will be:
update = dir * speed / sqrt(dir.x^2 + dir.y^2) * frameTime
characterPosition = characterPosition + update
Don't bother with angle calculations. Vector arithmetic is usually way more robust and expressive.
Mathematically (I won't provide code, sorry), assuming you can move in any direction on a two dimensional plane, from the top of my head it could be something like this (taken from old school geometry):
Having a speed, let's say 20 pixels per second (it could also be any other unit you chose, including in-game distance)
Having a polling system, where you have 2 main variables: the last known coords for the char (point A: Ax, Ay), the time of the last known update.
Having the time of the current update.
Having the coords for the destination (point B: Bx, By).
Figure out the current position of your character, which could be done like this (without converting from cartesian to polar coord system):
Figure out angle of movement: find the deltas for X and Y (Dx=Bx-Ax and Dy=By-Ay respectively), and use tangent to find the angle where Angle = tan-1(Dy/Dx).
Figure out the travelled distance (TD) from last poll to current poll where TD = speed * elapsed time
Figure out the coords for the new position (Cx and Cy) using sine and cosine, where travelled X is Tx=TD*sin(Angle) and travelled Y is Ty=TD*cos(Angle).
Now add the travelled distances to your original coords, and you get the current position. Where Cx=Ax+Tx and Cy=Ay+Ty.
How "smooth" movement is depends highly on the quality of your polling and somehow also on rounding for small distances.
Since u didn't provide us code I can just guess what exactly you are trying.
So I'll start with some basics.
I guess you are not just trying to make a program
but the basic movement of an 2d based game engine.
However a constant moving is based of an update method
that reads the player input about 30 times while rendering.
Example from my Projects:
public void run() {
boolean isRunning = true;
long lastNanoTime = System.nanoTime(),nowNanoTime = System.nanoTime();
final double secondAsNano = 1000000000.0;
final double sixtiethOfSecondAsNano = secondAsNano / 30.0;
int delta = 1;
while(isRunning){
nowNanoTime = System.nanoTime();
if(delta * sixtiethOfSecondAsNano + lastNanoTime < nowNanoTime){
update();
delta++;
if(delta == 30){
delta = 1;
lastNanoTime = nowNanoTime;
}
}
render();
}
}
public update(){
if(Input.moveUP) setPlayerY(getPlayerY() - 0.2);
if(Input.moveDOWN) setPlayerY(getPlayerY() + 0.2);
if(Input.moveRIGHT) setPlayerX(getPlayerX() + 0.2);
if(Input.moveLEFT) setPlayerX(getPlayerX() - 0.2);
}
I just cut it down to be easy readable so it might not work correct but it should explain you how its basically done.
I ended up figuring this out almost an hour after posting this. Sorry that my question was so vague, what I was looking for was the math behind what i was trying to do not the code itself but thanks to people who answered. Here is the solution I came up with in code :
if (q.get(0)[0][0] > q.get(0)[1][0]) {
if(q.get(0)[0][0] == q.get(0)[1][0]) {
currentLocation[0] -= 5 * Math.cos((Math.atan(((double) q.get(0)[0][1] - (double) q.get(0)[1][1]) / ((double) q.get(0)[0][0] - (double) q.get(0)[1][0]))));
currentLocation[1] += 5 * Math.sin((Math.atan(((double) q.get(0)[0][1] - (double) q.get(0)[1][1]) / ((double) q.get(0)[0][0] - (double) q.get(0)[1][0]))));
}
else{
currentLocation[0] -= 5 * Math.cos((Math.atan(((double) q.get(0)[0][1] - (double) q.get(0)[1][1]) / ((double) q.get(0)[0][0] - (double) q.get(0)[1][0]))));
currentLocation[1] -= 5 * Math.sin((Math.atan(((double) q.get(0)[0][1] - (double) q.get(0)[1][1]) / ((double) q.get(0)[0][0] - (double) q.get(0)[1][0]))));
}
} else {
if(q.get(0)[0][0] == q.get(0)[1][0]) {
currentLocation[0] += 5 * Math.cos((Math.atan(((double) q.get(0)[0][1] - (double) q.get(0)[1][1]) / ((double) q.get(0)[0][0] - (double) q.get(0)[1][0]))));
currentLocation[1] -= 5 * Math.sin((Math.atan(((double) q.get(0)[0][1] - (double) q.get(0)[1][1]) / ((double) q.get(0)[0][0] - (double) q.get(0)[1][0]))));
}
else{
currentLocation[0] += 5 * Math.cos((Math.atan(((double) q.get(0)[0][1] - (double) q.get(0)[1][1]) / ((double) q.get(0)[0][0] - (double) q.get(0)[1][0]))));
currentLocation[1] += 5 * Math.sin((Math.atan(((double) q.get(0)[0][1] - (double) q.get(0)[1][1]) / ((double) q.get(0)[0][0] - (double) q.get(0)[1][0]))));
}
}
I figured out a way to get the result I wanted though I probably over complicated it. q is an ArrayList that holds 2d arrays that are 2x2 [a/b][x/y]. and currentLocation a 2 index array that's just [x/y]. The result is the affect I wanted where it draws a line in (X units) a pass from point a to b in any direction at the same speed. This question was poorly worded and i'm sorry for wasting peoples time.

Converting distance (nautical miles) into degrees (Lat/Long)

I have entities in a simulation whose initial locations and paths are code using Java in decimal degrees. I need to scale the sensor radius (it's in nautical miles) and speed (nautical miles/hr) to match decimal degrees. The purpose is to visualize the sim in OpenMap and Google Earth.
I've seen How to convert Distance(miles) to degrees?, but the suggestions there don't work.
Any help is appreciated! I'm thinking it will involve using great circle distance formulas... but can't quite get it.
Ed Williams' Aviation Formula https://edwilliams.org/avform.htm is a good, and accessible, place to start. And I often reference http://movable-type.co.uk/scripts/latlong.html.
I am guessing that you need a vector of some sort (your question is a bit unclear).
What I use (in C, not Java) to calculate a fix-radial-distance is:
void polarToLatLong(double lat, double lon, double dist, double radial,
double *outlat, double *outlon) {
if (!dist) { // distance zero, so just return the point
*outlat = lat;
*outlon = lon;
}
else if (lat > 89.9999) { // North Pole singularity. Dist is in NM.
*outlat = 90 - dist / 60;
*outlon = fmod(radial + 180) - 180;
}
else { // normal case
double sinlat, coslon;
dist /= 3442; // = Earth's radius in nm (not WGS84!)
sinlat = Sin(lat) * cos(dist) + Cos(lat) * sin(dist) * Cos(radial);
*outlat = Arcsin(sinlat);
coslon = (cos(dist) - Sin(lat) * sinlat) / (Cos(lat) * Cos(*outlat));
*outlon = lon + (Sin(radial) >= 0 : -1 : 1) * Arccos(coslon);
}
}
In the above code Sin(), with an upper-case S, is just a wrapper of sin() for degrees:
#define CLAMP(a,x,b) MIN(MAX(a, x), b) // GTK+ GLib version slightly different
double Sin(double deg) {return sin(deg * (PI / 180));} // wrappers for degrees
double Cos(double deg) {return cos(deg * (PI / 180));}
double Arcsin(double x) {return asin(CLAMP(-1, x, 1)) * (180 / PI);}
double Arccos(double x) {return acos(CLAMP(-1, x, 1)) * (180 / PI);}

Categories

Resources