I am building and application where I need to switch an IMU on for around 5 seconds, and get a good orientation within the first 1-2 seconds. I am using the Madwick quaternion algorithm and it works relatively well. The problems is, if the sensor is rotated about 180 degrees while it being off, it takes a long time to find this new orientation.
Is there a way to initialise the quaternion values from the IMU sensor (instead of the standard 1, 0, 0, 0) to something close to the correct orientation? I am trying to do this by first calculating roll, pitch and yaw from the imu with the following:
pitch = 180 * atan2(accelX, sqrt(accelY*accelY + accelZ*accelZ))/PI;
roll = 180 * atan2(accelY, sqrt(accelX*accelX + accelZ*accelZ))/PI;
mag_x = magReadX*cos(pitch) + magReadY*sin(roll)*sin(pitch) + magReadZ*cos(roll)*sin(pitch)
mag_y = magReadY * cos(roll) - magReadZ * sin(roll)
yaw = 180 * atan2(-mag_y,mag_x)/M_PI;
and the converting these to quaternions with the following:
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
Quaternion q;
q.w = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;
However the heading seems very random. Please let me know if you know of a method to initialise the quaternions with semi-correct values.
Related
I have developed a custom CIWarpKernel which is the transposition of my Numbers model design as follows:
The cell for positive y is: SIN($A2×PI()÷2)×(−COS(B$1×PI())×($A2+1)÷$W$3+SIN($A2 ×PI()÷2)) where $A2 is y, B$1 is x and $W$3 is the center stretching factor.
The code is as follows:
kernel vec2 panoramaDistortion (vec2 center) {
float pi = 3.141592653589793;
vec2 t = destCoord();
float x = t.x / center.x - 1.0; // x ∈ -1...1
float y = t.y / center.y - 1.0; // y ∈ -1...1
float rx = x;
float delta = 50.0;
float siny = sin(y * pi / 2.0);
// See my model in Numbers: Aladdin PanoramPinch CIKernel file where $a2 = x and b$1 = y
// sin($a2×pi()÷2)×(−cos(b$1×pi())×($a2+1)÷$w$3+sin($a2 ×pi()÷$w$5))
// sin(y×pi()÷2)×(−cos(x×pi())×(y+1)÷$w$3+sin(y×pi()÷2))
// sin($a22×pi()÷2)×(−cos(b$1×pi())×abs(1−$a22)÷$w$3−sin($a22 ×pi()÷$w$5))
float ry =
y >= 0.0 ?
siny * (-cos(x * pi) * (y + 1.0) / delta + siny)
: siny * (-cos(x * pi) * (1.0 - y) / delta - siny)
;
return vec2(center.x * (rx + 1.0), center.y * (ry + 1.0));
}
The issue I have is that, though the transposition is 100% exact, I don't get the same result I have in my model. See the resulting warping of tiled-image:
How it comes I have the dilatation at the center y=0 and at the upper- and bottom- sides?
For information, my tiled-image is as follows:
Using Metal I am drawing line using Bezier Curves using four points. I am using nearly 1500 triangles for the lines. The line is Pixellated. How can i reduce pixellated.
vertex VertexOutBezier bezier_vertex(constant BezierParameters *allParams[[buffer(0)]],
constant GlobalParameters& globalParams[[buffer(1)]],
uint vertexId [[vertex_id]],
uint instanceId [[instance_id]])
{
float t = (float) vertexId / globalParams.elementsPerInstance;
rint(t);
BezierParameters params = allParams[instanceId];
float lineWidth = (1 - (((float) (vertexId % 2)) * 2.0)) * params.lineThickness;
float2 a = params.a;
float2 b = params.b;
float cx = distance(a , b);
float2 p1 = params.p1 * 3.0; // float2 p1 = params.p1 * 3.0;
float2 p2 = params.p2 * 3.0; // float2 p2 = params.p2 * 3.0;
float nt = 1.0f - t;
float nt_2 = nt * nt;
float nt_3 = nt_2 * nt;
float t_2 = t * t;
float t_3 = t_2 * t;
// Calculate a single point in this Bezier curve:
float2 point = a * nt_3 + p1 * nt_2 * t + p2 * nt * t_2 + b * t_3;
float2 tangent = -3.0 * a * nt_2 + p1 * (1.0 - 4.0 * t + 3.0 * t_2) + p2 * (2.0 * t - 3.0 * t_2) + 3 * b * t_2;
tangent = (float2(-tangent.y , tangent.x ));
VertexOutBezier vo;
vo.pos.xy = point + (tangent * (lineWidth / 2.0f));
vo.pos.zw = float2(0, 1);
vo.color = params.color;
return vo;
}
You need to enable MSAA (multisample anti-aliasing). How you do this depends on your exact Metal view configuration, but the easiest way is if you're using MTKView. To enable MSAA in an MTKView, all you have to do is:
metalView.sampleCount = 4
Then, when you configure your MTLRenderPipelineDescriptor before calling makeRenderPipelineState(), add the following:
pipelineDescriptor.sampleCount = 4
This should greatly improve the quality of your curves and reduce pixelation. It does come with a performance cost however, as the GPU has to do substantially more work to render your frame.
I am trying to use multiple iBeacons to track the user's location in iOS. I know this can be done (somewhat) using 3 beacons and trilateration, but I would like to do it with two (bilateration). I know that I will likely end up with two answers. Does anyone know of a simple way to accomplish this, given the (x,y) location of the beacons (relative to the room), and an averaged RSSI from each beacon?
I have this code for trilateration that I altered to objective-c from javascript:
- (CGPoint)getTrilaterationWithBeacon1:(BBBeacon *)beacon1 Beacon2:(BBBeacon *)beacon2 Beacon3:(BBBeacon *)beacon3 {
float xa = beacon1.x;
float ya = beacon1.y;
float xb = beacon2.x;
float yb = beacon2.y;
float xc = beacon3.x;
float yc = beacon3.y;
float ra = beacon1.distance;
float rb = beacon2.distance;
float rc = beacon3.distance;
float S = (pow(xc, 2.) - pow(xb, 2.) + pow(yc, 2.) - pow(yb, 2.) + pow(rb, 2.) - pow(rc, 2.)) / 2.0;
float T = (pow(xa, 2.) - pow(xb, 2.) + pow(ya, 2.) - pow(yb, 2.) + pow(rb, 2.) - pow(ra, 2.)) / 2.0;
float y = ((T * (xb - xc)) - (S * (xb - xa))) / (((ya - yb) * (xb - xc)) - ((yc - yb) * (xb - xa)));
float x = ((y * (ya - yb)) - T) / (xb - xa);
CGPoint point = CGPointMake(x, y);
return point;
}
So this is the code I ended up using, thanks to ChuckCottrill's suggestion that I look for a formula to calculate intersection of two circles. It is modified from a C version I found online here: http://paulbourke.net/geometry/circlesphere/tvoght.c
The results are somewhat inconsistent due to the inconsistency of the RSSI values returned from the iBeacons.
I will still need to add code to select the correct point somehow (it gives two results).
- (CGPoint)getBilaterationWithBeacon1:(BBBeacon *)beacon1 Beacon2:(BBBeacon *)beacon2 {
float x0 = beacon1.locationX;
float y0 = beacon1.locationY;
float r0 = beacon1.filteredDistance;
float x1 = beacon2.locationX;
float y1 = beacon2.locationY;
float r1 = beacon2.filteredDistance;
float a, dx, dy, d, h, rx, ry;
float x2, y2;
/* dx and dy are the vertical and horizontal distances between
* the circle centers.
*/
dx = x1 - x0;
dy = y1 - y0;
/* Determine the straight-line distance between the centers. */
d = sqrt((dy*dy) + (dx*dx));
/* Check for solvability. */
if (d > (r0 + r1)) {
/* no solution. circles do not intersect. */
return CGPointMake(-1, -1);
}
if (d < abs(r0 - r1)) {
/* no solution. one circle is contained in the other */
return CGPointMake(-1, -1);
}
/* 'point 2' is the point where the line through the circle
* intersection points crosses the line between the circle
* centers.
*/
/* Determine the distance from point 0 to point 2. */
a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
/* Determine the coordinates of point 2. */
x2 = x0 + (dx * a/d);
y2 = y0 + (dy * a/d);
/* Determine the distance from point 2 to either of the
* intersection points.
*/
h = sqrt((r0*r0) - (a*a));
/* Now determine the offsets of the intersection points from
* point 2.
*/
rx = -dy * (h/d);
ry = dx * (h/d);
/* Determine the absolute intersection points. */
float xi = x2 + rx;
float xi_prime = x2 - rx;
float yi = y2 + ry;
float yi_prime = y2 - ry;
CGPoint point1 = CGPointMake(xi, yi);
CGPoint point2 = CGPointMake(xi_prime, yi_prime);
//pick one
return point2;
}
I've got the following code to do a biliner interpolation from a matrix of 2D vectors, each cell has x and y values of the vector, and the function receives k and l indices telling the bottom-left nearest position in the matrix
// p[1] returns the interpolated values
// fieldLinePointsVerts the raw data array of fieldNumHorizontalPoints x fieldNumVerticalPoints
// only fieldNumHorizontalPoints matters to determine the index to access the raw data
// k and l horizontal and vertical indices of the point just bellow p[0] in the raw data
void interpolate( vertex2d* p, vertex2d* fieldLinePointsVerts, int fieldNumHorizontalPoints, int k, int l ) {
int index = (l * fieldNumHorizontalPoints + k) * 2;
vertex2d p11;
p11.x = fieldLinePointsVerts[index].x;
p11.y = fieldLinePointsVerts[index].y;
vertex2d q11;
q11.x = fieldLinePointsVerts[index+1].x;
q11.y = fieldLinePointsVerts[index+1].y;
index = (l * fieldNumHorizontalPoints + k + 1) * 2;
vertex2d q21;
q21.x = fieldLinePointsVerts[index+1].x;
q21.y = fieldLinePointsVerts[index+1].y;
index = ( (l + 1) * fieldNumHorizontalPoints + k) * 2;
vertex2d q12;
q12.x = fieldLinePointsVerts[index+1].x;
q12.y = fieldLinePointsVerts[index+1].y;
index = ( (l + 1) * fieldNumHorizontalPoints + k + 1 ) * 2;
vertex2d p22;
p22.x = fieldLinePointsVerts[index].x;
p22.y = fieldLinePointsVerts[index].y;
vertex2d q22;
q22.x = fieldLinePointsVerts[index+1].x;
q22.y = fieldLinePointsVerts[index+1].y;
float fx = 1.0 / (p22.x - p11.x);
float fx1 = (p22.x - p[0].x) * fx;
float fx2 = (p[0].x - p11.x) * fx;
vertex2d r1;
r1.x = fx1 * q11.x + fx2 * q21.x;
r1.y = fx1 * q11.y + fx2 * q21.y;
vertex2d r2;
r2.x = fx1 * q12.x + fx2 * q22.x;
r2.y = fx1 * q12.y + fx2 * q22.y;
float fy = 1.0 / (p22.y - p11.y);
float fy1 = (p22.y - p[0].y) * fy;
float fy2 = (p[0].y - p11.y) * fy;
p[1].x = fy1 * r1.x + fy2 * r2.x;
p[1].y = fy1 * r1.y + fy2 * r2.y;
}
Currently this code needs to be run every single frame in old iOS devices, say devices with arm6 processors
I've taken the numeric sub-indices from the wikipedia's equations http://en.wikipedia.org/wiki/Bilinear_interpolation
I'd accreciate any comments on optimization for performance, even plain asm code
This code should not be causing your slowdown if it's only run once per frame. However, if it's run multiple times per frame, it easily could be.
I'd run your app with a profiler to see where the true performance problem lies.
There is some room for optimization here: a) Certain index calculations could be factored out and re-used in subsequent calculations), b) You could dereference your fieldLinePointsVerts array to a pointer once and re-use that, instead of indexing it twice per index...
but in general those things won't help a great deal, unless this function is being called many, many times per frame. In which case every little thing will help.
I'm using the Core Data Framework to manage a set of accounts which also include geospatial (GPS) coordinate data for each account. How can I query against this data based on position of the device to get a list of accounts within x feet and list them in order of distance?
to get you started, here's a method i use in my iOS app that returns the distance in meters between two CLLocationCoordinate2D locations, assuming the Google Spherical Mercator Projection (if you want to use another projection, you can specify the appropriate flattening ratio value (f) and semi-major axis value (a). if you want the forward and backward azimuth values between the coordinates, you can uncomment and return the faz and baz values along with the distance by defining your own struct. this method can be used to add the distance to each of your 'account' objects and the current location being reported by your CLLocationManager object, then you could easily sort and filter an array of account objects based on their distances.
based on code by Gerald Evenden located here: http://article.gmane.org/gmane.comp.gis.proj-4.devel/3478
#define PI 3.141592653589793238462643
#define EPS 5e-14
#define DEG_TO_RAD 0.0174532925199432958
// returns the geodesic distance in meters between two coordinates based on the google spherical mercator projection.
- (int) geodesicDistanceFromCoordinate: (CLLocationCoordinate2D) fromCoord toCoordinate: (CLLocationCoordinate2D) toCoord {
double c, d, e, r, x, y, sa, cx, cy, cz, sx, sy, c2a, cu1, cu2, su1, tu1, tu2, ts, phi1, lam1, phi2, lam2, f, baz, faz, s, a;
phi1 = fromCoord.latitude * DEG_TO_RAD;
lam1 = fromCoord.longitude * DEG_TO_RAD;
phi2 = toCoord.latitude * DEG_TO_RAD;
lam2 = toCoord.longitude * DEG_TO_RAD;
f = 0; //google's spherical mercator projection has no flattening
a = 6378137; //earth's axis in meters used in google's projection
r = 1. - f;
tu1 = r * tan(phi1);
tu2 = r * tan(phi2);
cu1 = 1. / sqrt(tu1 * tu1 + 1.);
su1 = cu1 * tu1;
cu2 = 1. / sqrt(tu2 * tu2 + 1.);
ts = cu1 * cu2;
baz = ts * tu2;
faz = baz * tu1;
x = lam2 - lam1;
do {
sx = sin(x);
cx = cos(x);
tu1 = cu2 * sx;
tu2 = baz - su1 * cu2 * cx;
sy = sqrt(tu1 * tu1 + tu2 * tu2);
cy = ts * cx + faz;
y = atan2(sy, cy);
sa = ts * sx / sy;
c2a = -sa * sa + 1.;
cz = faz + faz;
if (c2a > 0.)
cz = -cz / c2a + cy;
e = cz * cz * 2. - 1.;
c = ((c2a * -3. + 4.) * f + 4.) * c2a * f / 16.;
d = x;
x = ((e * cy * c + cz) * sy * c + y) * sa;
x = (1. - c) * x * f + lam2 - lam1;
} while (fabs(d - x) > EPS);
//forward azimuth faz = atan2(tu1, tu2);
//backward azimuth baz = atan2(cu1 * sx, baz * cx - su1 * cu2) + PI;
x = sqrt((1. / r / r - 1.) * c2a + 1.) + 1.;
x = (x - 2.) / x;
c = (x * x / 4. + 1.) / (1. - x);
d = (x * .375 * x - 1.) * x;
s = ((((sy * sy * 4. - 3.) * (1. - e - e) * cz * d / 6. - e * cy) * d / 4. + cz) * sy * d + y) * c * r;
return (int)(s * a);
}