Yaw rate calculation from GPS Heading - c#

I have a dual GPS mounted on robot and getting data at 20Hz. I got heading data from GNTRA sentence which gives the heading from dual antenna. I am trying to get yaw rate of vehicle. I am having problem when I turn to left side of North. Normally, I am trying to get difference of two measurement and divide it with time interval. I am not good at trigonometry. For example, when I am driving to 5 degree of heading and making turn to left to 350 degree.I need to measure -15 degree/ time_interval. But I am measuring 345/time_interval with my code. I am making mistake and I could not figure out how to solve it. Can anybody help me? Thanks
public double calc_yawrate(heading)
{
mywatch.Stop();
double t = (double)mywatch.ElapsedMilliseconds / 1000;
mywatch.Reset();
mywatch.Start();
Speed = Speed / 3.6;
Speed = Math.Round(Speed, 2);
double yaw = heading;
double yawrate;
yawrate = (yaw- yawpast) / t;
yawpast = yaw;
}

It seems like you want the difference in range [-180°, 180°], then you can add/minus the value by 360° until it's in range.
var diff = yaw - yawpast;
if(diff > 180)
{
do
{
diff -= 360;
}
while(diff <= 180);
}
else if(diff < -180)
{
do
{
diff += 360;
}
while(diff >= -180);
}
yawrate = diff / t;

Related

How to use binary search on an animation curve to find time given value?

I'm using Unity's animation curves which unfortunately only have an Evaluate function that given time gives the value. I want to create a method that does the opposite of that. value gives time. I looked at this post: Unity get time by value but all the answers refer to inverting the curve which I think, and most comments agree, is cumbersome and error prone. I figured a better solution would be to use binary search. However, my current implementation sometimes fails to find the time.
For context the curve represents the velocity of a car as it accelerates. It is always monotonic increasing. So far this is what I've got:
float GetTimeOnCurve(AnimationCurve c, float speed)
{
if (speed < c.keys[0].value)
{
return c.keys[0].time;
}else if (speed > c.keys.Last().value)
{
return c.keys.Last().time;
}
float min = c.keys[0].time;
float max = c.keys.Last().time;
const float elipson = 0.001f;
while (min <= max)
{
float mid = (min + max) / 2;
if (speed >= c.Evaluate(mid) - elipson && speed <= c.Evaluate(mid) + elipson)
{
return mid;
}else if (speed < c.Evaluate(mid))
{
max = mid - elipson;
}
else
{
min = mid + elipson;
}
}
Debug.LogError("Speed not found");
return TopSpeed;
}
What can I do so that the time is always found?
It may fail on a steep curve, you should change the condition to
speed >= c.Evaluate(mid - elipson) && speed <= c.Evaluate(mid + elipson)

Accelerate rotation from 0 to x angle in x seconds in Unity

This is more of a math question than a coding question. I would like to reach for example an rotation angle of 90 in 1 second while speed is accelerating at constant value. My current version takes 1.4 seconds to reach the desired rotation angle, and it should reach it in 1 second. I believe that the reason for that is that it currently accelerates to speed of 90 in 1 second and not to rotation angle of 90. Since I am not that good in math, I have no idea how I need to adjust the acceleration calculation. I am unable to find any solution to this.
NOTE: I need to adjust the rotation angles manually, I am not able to use any existing functions, like for example transform.Rotate(), since in my complete version the rotation direction can change at any time and the rotation also has deceleration value.
This is a very simplified version of what I have (it only rotates the z axis to one direction and runs once on start):
private float accelerationInSeconds = 1;
private float targetAngle = 90f;
private float speed = 0;
private float axis = 1;
private bool rotate = true;
private float acceleration;
void Start() {
// Calculate acceleration (this calculation should be changed)
acceleration = targetAngle / accelerationInSeconds;
}
void Update() {
if (rotate) {
// Accelerate
speed += axis * (acceleration * Time.deltaTime);
// Calculate next rotation position
Vector3 rotationVector = transform.rotation.eulerAngles;
rotationVector.z += speed * Time.deltaTime;
// Rotate object
transform.rotation = Quaternion.Euler(rotationVector);
// Check if rotation has gone over the target angle
if (rotationVector.z >= targetAngle) {
rotationVector.z = targetAngle;
speed = 0;
rotate = false;
}
}
}
Thanks in advance for anyone who can help!
EDIT: Modified code to be more efficient. I can't use RotateTowards() since in my complete code I need to clamp the rotation between targetAngle and negative targetAngle. Hopefully this code is more efficient and performance friendly. But I still have not found a solution for my original math related question, which was the whole point of this question.
private float accelerationInSeconds = 1;
private float targetAngle = 90f;
private float speed = 0;
private float angle = 0;
private float axis = 1;
private bool rotate = true;
private float acceleration;
void Start() {
// Calculate acceleration (this calculation should be changed)
acceleration = targetAngle / accelerationInSeconds;
}
void Update() {
if (rotate) {
// Accelerate
speed += axis * (acceleration * Time.deltaTime);
// Calculate next rotation position
angle += speed * Time.deltaTime;
// Check if rotation has gone over the target angle
if (angle >= targetAngle) {
angle = targetAngle;
speed = 0;
rotate = false;
}
// Rotate object
transform.rotation = Quaternion.AngleAxis(angle, Vector3.forward);
}
}
I finally figured it out, thanks to Math section in StackExchange.
So the simple answer is this:
acceleration = 2 * targetAngle / Mathf.Pow(accelerationInSeconds, 2);
As was suggested before I would use a Coroutine. Coroutines are like temporary Update methods and often easier to control and maintain than doing stuff directly in Update.
// Flag to avoid concurrent routines
private bool isRotating;
public void Rotate(float targetAngle, float duration)
{
if(! isRotating) StartCoroutine (RotateRoutine(targetAngle, duration));
}
private IEnumerator RotateRoutine (float targetAngle, float duration)
{
// Just to be sure
if(isRotating) yield break;
// block concurrent routines
isRotating = true;
// Pre-calculate the start and end rotation
var start = transform.rotation;
var end = Quaternion.Euler(0, 0, targetAngle);
var timePassed = 0f;
while(timePassed < duration)
{
// This value will grow linear from 0 to 1 in exactly "duration" seconds
var x = timePassed / duration;
// TODO!
var y = MAGIC;
// Interpolate between the start and end rotation using given factor "y"
transform.rotation = Quaternion.Lerp(start, end, y);
// "pause" the routine here, render this frame
// and continue from here in the next frame
yield return null;
// Increase by the time passed since last frame
timePassed += Time.deltaTime;
}
// To be sure to end with clean values
transform.rotation = end;
// Allow next routine
isRotating = false;
}
So what do we have to fill in for MAGIC?
Basically it can be any mathematical function that maps given input 0 to 1 to 0 to 1.
There are multiple possibilities.
What you currently ask for is a linear growing speed. That means the resulting movement shall be quadratic! So we already know the Formular
var y = a * x * x + b;
We further know from your code that speed always starts from 0 -> b = 0. And the last step is pretty straight forward:
What value do we have to fill in so y goes from 0 to 1 at the same time that x goes from 0 to 1?
1 = a * 1 * 1 + 0;
=> a = 1!
So in your case it is simply
var y = x * x;
If you also want ease-out you could also simply use Mathf.Smoothstep which automatically adds ease-in and ease-out
var y = Mathf.SmoothStep(0, 1, x);
To make it even easier to control you could use an AnimationCurve and adjust the movement curve exactly to your needs in the Inspector
[SerializeField] private AnimationCurve curve;
The curve editor already comes with some preset curves like e.g. linear, logarithmic, exponential and eased-in/-out grow from 0 to 1!
And then use AnimationCurve.Evaluate to get the value (y) in the routine for a given input time (x).
var y = curve.Evaluate(x);

2d Billiard power physics

I have what I think is a mathematical problem with my power engine and I've spent the past 2 days trying to solve this. I am relatively new to programming and am in the process of developing a 2D top-down billiard game in Unity.
My code is attached below, but essentially the player will click on the ball and drag back to produce power. Once they reach the desired power and let go the ball will launch in the desired direction at the desired power. For the sake of this example let's say that I set maxPower to 100, and the player object's linear drag and the tile drag remains constant. In the following example scenarios, all shots are at full power, and I'm shooting from the bottom left of my table:
I shoot a purely x-axis shot - the ball is shot at 100 power to the right
I shoot a purely y-axis shot - the ball is shot at 100 power upwards
I shoot at a 45 degree angle - how can I calculate what happens here?
If you look at the code below, I am using AddForce to push the ball on the x and y axis once I release the mouse. I'm tracking the distance of each shot made at a variety of angles and I've tried the following power management:
Splitting the power between x and y axis - this ends up produces a much weaker shot than a pure x/y axis shot (50 power per axis)
Giving each axis the full power - this ends up producing a much more powerful shot (100 power per axis)
Feeling like Goldilocks, I tried to make this juuuuust right and compute this using the Pythagorean theorem, and this is where I am struggling.
Any help would be much appreciated.
Code below - all variables are defined, I've just removed irrelevant code.
public float maxForce = 100;
public float forceMultiplier = 2
void Awake() {
startPosition = transform.position;
}
void Update () {
/// Tracks distance (start vs current)
distance = Vector3.Distance(transform.position,startPosition);
//On Click
if (Input.GetMouseButtonDown (0)) {
startPosX = Input.mousePosition.x;
startPosY = Input.mousePosition.y;
}
/// While being clicked
if (Input.GetMouseButton (0)) {
/// Determines force to be applied based on mouse drag
current_xForce = (startPosX - Input.mousePosition.x);
current_yForce = (startPosY - Input.mousePosition.y);
// Stores the original drag angle
yCheck = current_yForce;
xCheck = current_xForce;
/// if current x/y force is greater than maxForce, set to maxForce
if (current_yForce > 0 && current_yForce > maxForce)
current_yForce = maxForce;
if (current_yForce < 0 && current_yForce < -maxForce)
current_yForce = -maxForce;
if (current_xForce > 0 && current_xForce > maxForce)
current_xForce = maxForce;
if (current_xForce < 0 && current_xForce < -maxForce)
current_xForce = -maxForce;
// Determines the % of x/y while aiming
current_xPercentage = Mathf.Abs (current_xForce) / (Mathf.Abs (current_xForce) + Mathf.Abs (current_yForce));
current_yPercentage = Mathf.Abs (current_yForce) / (Mathf.Abs (current_xForce) + Mathf.Abs (current_yForce));
// Decides the Power bar% - power determined by highest powered axis (may need improvement)
current_powerPercent = Mathf.Max (Mathf.Abs (current_xForce)/maxForce, Mathf.Abs (current_yForce)/maxForce);
// get angle from start position to relative mouse position... add relative mouse position NOT WORKING... need degrees
shotAngle = Vector2.Angle(startPosition,Input.mousePosition);
}
// Mouse button released
if (Input.GetMouseButtonUp (0)) {
/// Only shots with greater than % of total power count - allows you to cancel shots
if (current_powerPercent >= 0.10) {
/// Increase force by public multiplier
xForce = current_xForce * forceMultiplier ;
yForce = current_yForce * forceMultiplier ;
/// Use % of absolute xCheck to determine shot angle
float xForcePercent = Mathf.Abs (xCheck) / (Mathf.Abs (yCheck) + Mathf.Abs (xCheck));
float yForcePercent = Mathf.Abs (yCheck) / (Mathf.Abs (yCheck) + Mathf.Abs (xCheck));
/// Adds force to x/y based off xForce and the shot angle
float x_addForce = xForce * xForcePercent;
float y_addForce = yForce * yForcePercent;
/// Attempt at a-squared + b-squared = c-squared.... this is supposed to return c
powerVar = Mathf.Sqrt(Mathf.Pow (x_addForce*xForcePercent, 2) + Mathf.Pow (y_addForce*yForcePercent, 2));
/// Applies the force to the player object.. .for each axis I take powerVar * angle * sign (to adjust for negative values)
GetComponent<Rigidbody2D>().AddForce(new Vector2(powerVar*xForcePercent*Mathf.Sign(x_addForce), powerVar*yForcePercent*Mathf.Sign(y_addForce)));
yForce = 0;
xForce = 0;
startPosX = 0;
startPosY = 0;
distance = 0;
startPosition = transform.position; /// resets Distance counter
}
}
}
The Pythagorean theorem dictates that in a right triangle the following is true:
c * c = a * a + b * b
Where c is the hypotenuse. Now, in your case c is 100 and the angle between c and a or b is 45°. This means you are equally "splitting" c between a and b. Ok, let's do that:
a = b, therefore:
c * c = a * a + a * a, which means
c * c = 2 * a * a
If we take the square root on both sides:
c = sqrt(2) * a, and therefore
a = c / sqrt(2)
You will see this same relation written may times in the following form:
a = sqrt(2)/2 * c
And there you go. In your case if c is 100 then a and b will be 70,711.
You can generalize this for any angle if you use trigonometric functions (based on the Pythagorean theorem). For any angle d between c and a, the following is true:
a = c * cos(d)
b = c * sin(d)
Make sure you get your units right if you are going to use trigonometric functions. Angles are normally given in radians, not degrees, the relation between the two is:
180 degrees = pi radians

calculating rotation delta based off initial angle and current angle

I'm looking to track the rotation delta (y axis) between two readings.
There's a game object that I read an initial angle from when it starts moving.
Then I have another variable that reads the live angle during the update loop.
I want the delta value to be agnostic of whether it's negative or positive, essentially I'm going to apply the same updates whether the object rotation 20 degrees to the left or right.
EDIT:
I want the delta value to be the smallest angle, so the maximum it could be is 180, before counting back down to 0.
EXAMPLE:
If my initFaceAngle == 5 and currentFaceAngle == 355 then myAngle == 10
void Start()
{
initFaceAngle = hmd.transform.rotation.eulerAngles.y;
}
void update()
{
currentFaceAngle = hmd.transform.rotation.eulerAngles.y;
// My terrible first attempt... look ma' I can math.
float myAngle = (float)Math.Abs(initFaceAngle - currentFaceAngle);
}
Obviously my calculation won't work because getting the difference between two angles doesn't take into account the 360 degree. So I figured I need some pie (pi) on this, but outside of being an impressive pie (pi) eater I don't have a clue how to invoke its magical math powers.
What formula do I need to use to capture the delta?
How can I make it read the same whether rotating left or right?
Because you only want the angle between the initial and current position, it's actually no math involved, only a simple check to see which value is the greatest.
void update()
{
currentFaceAngle = hmd.transform.rotation.eulerAngles.y;
float myAngle;
if(initFaceAngle > currentFaceAngle)
myAngle = initFaceAngle - currentFaceAngle;
else
myAngle = currentFaceAngle - initFaceAngle;
if(myAngle > 180)
myAngle = 360 - myangle;
}
I think I've worked out a solution, not sure if it's the most graceful but the output values look correct.
void Start()
{
initFaceAngle = hmd.transform.rotation.eulerAngles.y;
}
void update()
{
currentFaceAngle = hmd.transform.rotation.eulerAngles.y;
float myAngle = getDeltaAngle(initFaceAngle, currentFaceAngle);
}
public static float getDeltaAngle(float a, float b)
{
float x;
float y;
if (a > b)
{
x = a;
y = b;
}
else
{
x = b;
y = a;
}
if (x - y < 180)
{
x = x - y;
}
else
{
x = (360 - x) + y;
}
return x;
}

Rotation direction continuos check

I'm working on a moltitouch application in actionscript 3, I'm also porting it in C#, Basically, im working on a Knob, that can be rotated with a finger, what i would like to achieve is that rotating CW or CCW i can have continuos direction, instead everytime angle is passing by 180 I got an inversion of direction, any hint ?
Which way can be detected a continuos rotation direction ?
this is the code I'm uding to detect direction:
private function findDirection(currentAngle : Number, targetAngle : Number) : int
{
currentAngle = refineAngle(currentAngle);
targetAngle = refineAngle(targetAngle);
if (targetAngle < 0)
{
targetAngle += (Math.PI * 2);
}
if (currentAngle < 0)
{
currentAngle += (Math.PI * 2);
}
if (targetAngle < currentAngle)
{
targetAngle += (Math.PI * 2);
}
if (targetAngle - currentAngle <= Math.PI)
{
return 1;
}
else
{
return -1;
}
}
private function refineAngle(angle : Number) : Number
{
return angle * Math.PI / 180;
}
Maybe this helps. The variable continuousAngle will track the total knob turning performed, i.e. turning the knob twice counterclockwise will get you to 720. Then turning it three times clockwise takes you back down to -360. Everything else should be easy to derive - limiting the minimum and maximum values, making the value wrap around, scale the value to for example 1 per turn or whatever else you want.
var lastAngle = 0;
var continuousAngle = 0;
function HandleDown(angle)
{
lastAngle = angle;
}
function HandleMove(angle)
{
// The orientation change in degrees of the knob since the last event with
// a range of [-180;+180). A positive value indicates counterclockwise, a
// negative value clockwise turning.
var change = (360 + angle - lastAngle) % 360;
if (change >= 180)
{
change -= 360;
}
// It may also be a good idea to not update continuousAngle if the absolute
// value of change is larger than say 10°, 20° or 40° because such large
// changes may indicate some kind of glitch like the user moving straight
// across the knob. But I am not sure and 20 is just a random guess.
if (Math.Abs(change) <= 20)
{
continuousAngle += change;
}
lastAngle = angle;
}
For floating point numbers the reminder can be calculated using Math.IEEEReminder instead of the remainder operator %. The linked page also shows how to implement this function yourself if it is not available in your language.

Categories