How to apply a filter on CMRotationMatrix using a CADisplayLink - objective-c

How to apply a filter on CMRotationMatrix? maybe kalman filter. I need fix the noise of CMRotationMatrix (transformFromCMRotationMatrix), to get a linear values of result matrix
This matrix values will be convert to XYZ, in my case I'm simulate 3D on 2D screen like that:
// Casting matrix to x, y
vec4f_t v;
multiplyMatrixAndVector(v, projectionCameraTransform, boxMatrix);
float x = (v[0] / v[3] + 1.0f) * 0.5f;
float y = (v[1] / v[3] + 1.0f) * 0.5f;
CGPointMake(x * self.bounds.size.width, self.bounds.size.height - (y * self.bounds.size.height));
code:
// define variable
mat4f_t cameraTransform;
// start the display link loop
- (void)startDisplayLink
{
displayLink = [CADisplayLink displayLinkWithTarget:self selector:#selector(onDisplayLink:)];
[displayLink setFrameInterval:1];
[displayLink addToRunLoop:[NSRunLoop currentRunLoop] forMode:NSDefaultRunLoopMode];
}
// stop the display link loop
- (void)stopDisplayLink
{
[displayLink invalidate];
displayLink = nil;
}
// event of display link
- (void)onDisplayLink:(id)sender
{
CMDeviceMotion *d = motionManager.deviceMotion;
if (d != nil) {
CMRotationMatrix r = d.attitude.rotationMatrix;
transformFromCMRotationMatrix(cameraTransform, &r);
[self setNeedsDisplay];
}
}
// function trigger before [self setNeedDisplay];
void transformFromCMRotationMatrix(vec4f_t mout, const CMRotationMatrix *m)
{
mout[0] = (float)m->m11;
mout[1] = (float)m->m21;
mout[2] = (float)m->m31;
mout[3] = 0.0f;
mout[4] = (float)m->m12;
mout[5] = (float)m->m22;
mout[6] = (float)m->m32;
mout[7] = 0.0f;
mout[8] = (float)m->m13;
mout[9] = (float)m->m23;
mout[10] = (float)m->m33;
mout[11] = 0.0f;
mout[12] = 0.0f;
mout[13] = 0.0f;
mout[14] = 0.0f;
mout[15] = 1.0f;
}
// Matrix-vector and matrix-matricx multiplication routines
void multiplyMatrixAndVector(vec4f_t vout, const mat4f_t m, const vec4f_t v)
{
vout[0] = m[0]*v[0] + m[4]*v[1] + m[8]*v[2] + m[12]*v[3];
vout[1] = m[1]*v[0] + m[5]*v[1] + m[9]*v[2] + m[13]*v[3];
vout[2] = m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14]*v[3];
vout[3] = m[3]*v[0] + m[7]*v[1] + m[11]*v[2] + m[15]*v[3];
}

In general I would distinguish between improving the signal noise ratio and smooting the signal.
Signal Improvement
If you really want to be better than Apple's Core Motion which has already a sensor fusion algorithm implemented, stay prepared for a long term project with an uncertain outcome. In this case you would be better off to take the raw accelerometer and gyro signals to build your own sensor fusion algorithm but you have to care about a lot of problems like drift, hardware dependency of different iPhone versions, hardware differences of the sensors within same generation, ... So my advice: try everything to avoid it.
Smoothing
This just means interpolating two or more signals and building kind of average. I don't know about any suitable approach to use for rotation matrices directly (maybe there is one) but you can use quaternions instead (more resources: OpenGL Tutorial Using Quaternions to represent rotation or Quaternion FAQ).
The resulting quaternion of such an interpolation can be multiplied with your vector to get the projection similarly to the matrix way (you may look at Finding normal vector to iOS device for more information).
Interpolation between two unit quaternions representing rotations can be accomplished with Slerp. In practice you will use what is described as Geometric Slerp in Wikipedia. If you have two points in time t1 and t2 and the corresponding quaternions q1 and q2 and the angular distance omega between them, the formula is:
q'(q1, q2, t) = sin((1- t) * omega) / sin(omega) * q0 + sin(t * omega) / sin(omega) * q1
t should be 0.5 because you want the average between both rotations. Omega can be calculated by the dot product:
cos(omega) = q1.q2 = w1*w2 + x1*x2 + y1*y2 + z1*z2
If this approach using two quaternions still doesn't match your needs, you can repeat this by using slerp (slerp (q1, q2), slerp (q3, q4)). Some notes:
From a performance point fo view it's not that cheap to perform three sin and one arccos call in your run loop 1/frequency times per second. Thus you should avoid using too many points
In your case all signals are close to each other especially when using high sensor frequencies. You have to take care about angles that are very small and let 1/sin(omega) explode. In this case set sin(x) ≈ x
Like in other filters like low-pass filter the more points in time you use the more time delay you get. So if you have frequency f you will get about 0.5/f sec delay when using two points and 1.5/f for the double slerp.
If something appears weird, check that your resulting quaternions are unit quaternions i.e. ||q|| = 1
If you are running into performance issues you might have a look at Hacking Quaternions
The C++ project pbrt at github contains a quaternion class to get some inspiration from.

Related

Convert from latitude, longitude to x, y

I want to convert GPS location (latitude, longitude) into x,y coordinates.
I found many links about this topic and applied it, but it doesn't give me the correct answer!
I am following these steps to test the answer:
(1) firstly, i take two positions and calculate the distance between them using maps.
(2) then convert the two positions into x,y coordinates.
(3) then again calculate distance between the two points in the x,y coordinates
and see if it give me the same result in point(1) or not.
one of the solution i found the following, but it doesn't give me correct answer!
latitude = Math.PI * latitude / 180;
longitude = Math.PI * longitude / 180;
// adjust position by radians
latitude -= 1.570795765134; // subtract 90 degrees (in radians)
// and switch z and y
xPos = (app.radius) * Math.sin(latitude) * Math.cos(longitude);
zPos = (app.radius) * Math.sin(latitude) * Math.sin(longitude);
yPos = (app.radius) * Math.cos(latitude);
also i tried this link but still not work with me well!
any help how to convert from(latitude, longitude) to (x,y) ?
Thanks,
No exact solution exists
There is no isometric map from the sphere to the plane. When you convert lat/lon coordinates from the sphere to x/y coordinates in the plane, you cannot hope that all lengths will be preserved by this operation. You have to accept some kind of deformation. Many different map projections do exist, which can achieve different compromises between preservations of lengths, angles and areas. For smallish parts of earth's surface, transverse Mercator is quite common. You might have heard about UTM. But there are many more.
The formulas you quote compute x/y/z, i.e. a point in 3D space. But even there you'd not get correct distances automatically. The shortest distance between two points on the surface of the sphere would go through that sphere, whereas distances on the earth are mostly geodesic lengths following the surface. So they will be longer.
Approximation for small areas
If the part of the surface of the earth which you want to draw is relatively small, then you can use a very simple approximation. You can simply use the horizontal axis x to denote longitude λ, the vertical axis y to denote latitude φ. The ratio between these should not be 1:1, though. Instead you should use cos(φ0) as the aspect ratio, where φ0 denotes a latitude close to the center of your map. Furthermore, to convert from angles (measured in radians) to lengths, you multiply by the radius of the earth (which in this model is assumed to be a sphere).
x = r λ cos(φ0)
y = r φ
This is simple equirectangular projection. In most cases, you'll be able to compute cos(φ0) only once, which makes subsequent computations of large numbers of points really cheap.
I want to share with you how I managed the problem. I've used the equirectangular projection just like #MvG said, but this method gives you X and Y positions related to the globe (or the entire map), this means that you get global positions. In my case, I wanted to convert coordinates in a small area (about 500m square), so I related the projection point to another 2 points, getting the global positions and relating to local (on screen) positions, just like this:
First, I choose 2 points (top-left and bottom-right) around the area where I want to project, just like this picture:
Once I have the global reference area in lat and lng, I do the same for screen positions. The objects containing this data are shown below.
//top-left reference point
var p0 = {
scrX: 23.69, // Minimum X position on screen
scrY: -0.5, // Minimum Y position on screen
lat: -22.814895, // Latitude
lng: -47.072892 // Longitude
}
//bottom-right reference point
var p1 = {
scrX: 276, // Maximum X position on screen
scrY: 178.9, // Maximum Y position on screen
lat: -22.816419, // Latitude
lng: -47.070563 // Longitude
}
var radius = 6371; //Earth Radius in Km
//## Now I can calculate the global X and Y for each reference point ##\\
// This function converts lat and lng coordinates to GLOBAL X and Y positions
function latlngToGlobalXY(lat, lng){
//Calculates x based on cos of average of the latitudes
let x = radius*lng*Math.cos((p0.lat + p1.lat)/2);
//Calculates y based on latitude
let y = radius*lat;
return {x: x, y: y}
}
// Calculate global X and Y for top-left reference point
p0.pos = latlngToGlobalXY(p0.lat, p0.lng);
// Calculate global X and Y for bottom-right reference point
p1.pos = latlngToGlobalXY(p1.lat, p1.lng);
/*
* This gives me the X and Y in relation to map for the 2 reference points.
* Now we have the global AND screen areas and then we can relate both for the projection point.
*/
// This function converts lat and lng coordinates to SCREEN X and Y positions
function latlngToScreenXY(lat, lng){
//Calculate global X and Y for projection point
let pos = latlngToGlobalXY(lat, lng);
//Calculate the percentage of Global X position in relation to total global width
pos.perX = ((pos.x-p0.pos.x)/(p1.pos.x - p0.pos.x));
//Calculate the percentage of Global Y position in relation to total global height
pos.perY = ((pos.y-p0.pos.y)/(p1.pos.y - p0.pos.y));
//Returns the screen position based on reference points
return {
x: p0.scrX + (p1.scrX - p0.scrX)*pos.perX,
y: p0.scrY + (p1.scrY - p0.scrY)*pos.perY
}
}
//# The usage is like this #\\
var pos = latlngToScreenXY(-22.815319, -47.071718);
$point = $("#point-to-project");
$point.css("left", pos.x+"em");
$point.css("top", pos.y+"em");
As you can see, I made this in javascript, but the calculations can be translated to any language.
P.S. I'm applying the converted positions to an HTML element whose id is "point-to-project". To use this piece of code on your project, you shall create this element (styled as position absolute) or change the "usage" block.
Since this page shows up on top of google while i searched for this same problem, I would like to provide a more practical answers. The answer by MVG is correct but rather theoratical.
I have made a track plotting app for the fitbit ionic in javascript. The code below is how I tackled the problem.
//LOCATION PROVIDER
index.js
var gpsFix = false;
var circumferenceAtLat = 0;
function locationSuccess(pos){
if(!gpsFix){
gpsFix = true;
circumferenceAtLat = Math.cos(pos.coords.latitude*0.01745329251)*111305;
}
pos.x:Math.round(pos.coords.longitude*circumferenceAtLat),
pos.y:Math.round(pos.coords.latitude*110919),
plotTrack(pos);
}
plotting.js
plotTrack(position){
let x = Math.round((this.segments[i].start.x - this.bounds.minX)*this.scale);
let y = Math.round(this.bounds.maxY - this.segments[i].start.y)*this.scale; //heights needs to be inverted
//redraw?
let redraw = false;
//x or y bounds?
if(position.x>this.bounds.maxX){
this.bounds.maxX = (position.x-this.bounds.minX)*1.1+this.bounds.minX; //increase by 10%
redraw = true;
}
if(position.x<this.bounds.minX){
this.bounds.minX = this.bounds.maxX-(this.bounds.maxX-position.x)*1.1;
redraw = true;
};
if(position.y>this.bounds.maxY){
this.bounds.maxY = (position.y-this.bounds.minY)*1.1+this.bounds.minY; //increase by 10%
redraw = true;
}
if(position.y<this.bounds.minY){
this.bounds.minY = this.bounds.maxY-(this.bounds.maxY-position.y)*1.1;
redraw = true;
}
if(redraw){
reDraw();
}
}
function reDraw(){
let xScale = device.screen.width / (this.bounds.maxX-this.bounds.minX);
let yScale = device.screen.height / (this.bounds.maxY-this.bounds.minY);
if(xScale<yScale) this.scale = xScale;
else this.scale = yScale;
//Loop trough your object to redraw all of them
}
For completeness I like to add my python adaption of #allexrm code which worked really well. Thanks again!
radius = 6371 #Earth Radius in KM
class referencePoint:
def __init__(self, scrX, scrY, lat, lng):
self.scrX = scrX
self.scrY = scrY
self.lat = lat
self.lng = lng
# Calculate global X and Y for top-left reference point
p0 = referencePoint(0, 0, 52.526470, 13.403215)
# Calculate global X and Y for bottom-right reference point
p1 = referencePoint(2244, 2060, 52.525035, 13.405809)
# This function converts lat and lng coordinates to GLOBAL X and Y positions
def latlngToGlobalXY(lat, lng):
# Calculates x based on cos of average of the latitudes
x = radius*lng*math.cos((p0.lat + p1.lat)/2)
# Calculates y based on latitude
y = radius*lat
return {'x': x, 'y': y}
# This function converts lat and lng coordinates to SCREEN X and Y positions
def latlngToScreenXY(lat, lng):
# Calculate global X and Y for projection point
pos = latlngToGlobalXY(lat, lng)
# Calculate the percentage of Global X position in relation to total global width
perX = ((pos['x']-p0.pos['x'])/(p1.pos['x'] - p0.pos['x']))
# Calculate the percentage of Global Y position in relation to total global height
perY = ((pos['y']-p0.pos['y'])/(p1.pos['y'] - p0.pos['y']))
# Returns the screen position based on reference points
return {
'x': p0.scrX + (p1.scrX - p0.scrX)*perX,
'y': p0.scrY + (p1.scrY - p0.scrY)*perY
}
pos = latlngToScreenXY(52.525607, 13.404572);
pos['x] and pos['y] contain the translated x & y coordinates of the lat & lng (52.525607, 13.404572)
I hope this is helpful for anyone looking like me for the proper solution to the problem of translating lat lng into a local reference coordinate system.
Best
Its better to convert to utm coordinates, and treat that as x and y.
import utm
u = utm.from_latlon(12.917091, 77.573586)
The result will be (779260.623156606, 1429369.8665238516, 43, 'P')
The first two can be treated as x,y coordinates, the 43P is the UTM Zone, which can be ignored for small areas (width upto 668 km).

The math behind Apple's Speak here example

I have a question regarding the math that Apple is using in it's speak here example.
A little background: I know that average power and peak power returned by the AVAudioRecorder and AVAudioPlayer is in dB. I also understand why the RMS power is in dB and that it needs to be converted into amp using pow(10, (0.5 * avgPower)).
My question being:
Apple uses this formula to create it's "Meter Table"
MeterTable::MeterTable(float inMinDecibels, size_t inTableSize, float inRoot)
: mMinDecibels(inMinDecibels),
mDecibelResolution(mMinDecibels / (inTableSize - 1)),
mScaleFactor(1. / mDecibelResolution)
{
if (inMinDecibels >= 0.)
{
printf("MeterTable inMinDecibels must be negative");
return;
}
mTable = (float*)malloc(inTableSize*sizeof(float));
double minAmp = DbToAmp(inMinDecibels);
double ampRange = 1. - minAmp;
double invAmpRange = 1. / ampRange;
double rroot = 1. / inRoot;
for (size_t i = 0; i < inTableSize; ++i) {
double decibels = i * mDecibelResolution;
double amp = DbToAmp(decibels);
double adjAmp = (amp - minAmp) * invAmpRange;
mTable[i] = pow(adjAmp, rroot);
}
}
What are all the calculations - or rather, what do each of these steps do? I think that mDecibelResolution and mScaleFactor are used to plot 80dB range over 400 values (unless I'm mistaken). However, what's the significance of inRoot, ampRange, invAmpRange and adjAmp? Additionally, why is the i-th entry in the meter table "mTable[i] = pow(adjAmp, rroot);"?
Any help is much appreciated! :)
Thanks in advance and cheers!
It's been a month since I've asked this question, and thanks, Geebs, for your response! :)
So, this is related to a project that I've been working on, and the feature that is based on this was implemented about 2 days after asking that question. Clearly, I've slacked off on posting a closing response (sorry about that). I posted a comment on Jan 7, as well, but circling back, seems like I had a confusion with var names. >_<. Thought I'd give a full, line by line answer to this question (with pictures). :)
So, here goes:
//mDecibelResolution is the "weight" factor of each of the values in the meterTable.
//Here, the table is of size 400, and we're looking at values 0 to 399.
//Thus, the "weight" factor of each value is minValue / 399.
MeterTable::MeterTable(float inMinDecibels, size_t inTableSize, float inRoot)
: mMinDecibels(inMinDecibels),
mDecibelResolution(mMinDecibels / (inTableSize - 1)),
mScaleFactor(1. / mDecibelResolution)
{
if (inMinDecibels >= 0.)
{
printf("MeterTable inMinDecibels must be negative");
return;
}
//Allocate a table to store the 400 values
mTable = (float*)malloc(inTableSize*sizeof(float));
//Remember, "dB" is a logarithmic scale.
//If we have a range of -160dB to 0dB, -80dB is NOT 50% power!!!
//We need to convert it to a linear scale. Thus, we do pow(10, (0.05 * dbValue)), as stated in my question.
double minAmp = DbToAmp(inMinDecibels);
//For the next couple of steps, you need to know linear interpolation.
//Again, remember that all calculations are on a LINEAR scale.
//Attached is an image of the basic linear interpolation formula, and some simple equation solving.
//As per the image, and the following line, (y1 - y0) is the ampRange -
//where y1 = maxAmp and y0 = minAmp.
//In this case, maxAmp = 1amp, as our maxDB is 0dB - FYI: 0dB = 1amp.
//Thus, ampRange = (maxAmp - minAmp) = 1. - minAmp
double ampRange = 1. - minAmp;
//As you can see, invAmpRange is the extreme right hand side fraction on our image's "Step 3"
double invAmpRange = 1. / ampRange;
//Now, if we were looking for different values of x0, x1, y0 or y1, simply substitute it in that equation and you're good to go. :)
//The only reason we were able to get rid of x0 was because our minInterpolatedValue was 0.
//I'll come to this later.
double rroot = 1. / inRoot;
for (size_t i = 0; i < inTableSize; ++i) {
//Thus, for each entry in the table, multiply that entry with it's "weight" factor.
double decibels = i * mDecibelResolution;
//Convert the "weighted" value to amplitude using pow(10, (0.05 * decibelValue));
double amp = DbToAmp(decibels);
//This is linear interpolation - based on our image, this is the same as "Step 3" of the image.
double adjAmp = (amp - minAmp) * invAmpRange;
//This is where inRoot and rroot come into picture.
//Linear interpolation gives you a "straight line" between 2 end-points.
//rroot = 0.5
//If I raise a variable, say myValue by 0.5, it is essentially taking the square root of myValue.
//So, instead of getting a "straight line" response, by storing the square root of the value,
//we get a curved response that is similar to the one drawn in the image (note: not to scale).
mTable[i] = pow(adjAmp, rroot);
}
}
Response Curve image: As you can see, the "Linear curve" is not exactly a curve. >_<
Hope this helps the community in some way. :)
No expert, but based on physics and math:
Assume the max amplitude is 1 and minimum is 0.0001 [corresponding to -80db, which is what min db value is set to in the apple example : #define kMinDBvalue -80.0 in AQLevelMeter.h]
minAmp is the minimum amplitude = 0.0001 for this example
Now, all that is being done is the amplitudes in multiples of the decibel resolution are being adjusted against the minimum amplitude:
adjusted amplitude = (amp-minamp)/(1-minamp)
This makes the range of the adjusted amplitude = 0 to 1 instead of 0.0001 to 1 (if that was desired).
inRoot is set to 2 here. rroot=1/2 - raising to power 1/2 is square root. from apple's file:
// inRoot - this controls the curvature of the response. 2.0 is square root, 3.0 is cube root. But inRoot doesn't have to be integer valued, it could be 1.8 or 2.5, etc.
Essentially gives you a response between 0 and 1 again, and the curvature of that varies based on what value you set for inRoot.

Can't correctly rotate cylinder in openGL to desired position

I've got a little objective-c utility program that renders a convex hull. (This is to troubleshoot a bug in another program that calculates the convex hull in preparation for spatial statistical analysis). I'm trying to render a set of triangles, each with an outward-pointing vector. I can get the triangles without problems, but the vectors are driving me crazy.
I'd like the vectors to be simple cylinders. The problem is that I can't just declare coordinates for where the top and bottom of the cylinders belong in 3D (e.g., like I can for the triangles). I have to make them and then rotate and translate them from their default position along the z-axis. I've read a ton about Euler angles, and angle-axis rotations, and quaternions, most of which is relevant, but not directed at what I need: most people have a set of objects and then need to rotate the object in response to some input. I need to place the object correctly in the 3D "scene".
I'm using the Cocoa3DTutorial classes to help me out, and they work great as far as I can tell, but the rotation bit is killing me.
Here is my current effort. It gives me cylinders that are located correctly, but all point along the z-axis (as in this image:. We are looking in the -z direction. The triangle poking out behind is not part of the hull; for testing/debugging. The orthogonal cylinders are coordinate axes, more or less, and the spheres are to make sure the axes are located correctly, since I have to use rotation to place those cylinders correctly. And BTW, when I use that algorithm, the out-vectors fail as well, although in a different way, coming out normal to the planes, but all pointing in +z instead of some in -z)
from Render3DDocument.m:
// Make the out-pointing vector
C3DTCylinder *outVectTube;
C3DTEntity *outVectEntity;
Point3DFloat *sideCtr = [thisSide centerOfMass];
outVectTube = [C3DTCylinder cylinderWithBase: tubeRadius top: tubeRadius height: tubeRadius*10 slices: 16 stacks: 16];
outVectEntity = [C3DTEntity entityWithStyle:triColor
geometry:outVectTube];
Point3DFloat *outVect = [[thisSide inVect] opposite];
Point3DFloat *unitZ = [Point3DFloat pointWithX:0 Y:0 Z:1.0f];
Point3DFloat *rotAxis = [outVect crossWith:unitZ];
double rotAngle = [outVect angleWith:unitZ];
[outVectEntity setRotationX: rotAxis.x
Y: rotAxis.y
Z: rotAxis.z
W: rotAngle];
[outVectEntity setTranslationX:sideCtr.x - ctrX
Y:sideCtr.y - ctrY
Z:sideCtr.z - ctrZ];
[aScene addChild:outVectEntity];
(Note that Point3DFloat is basically a vector class, and that a Side (like thisSide) is a set of four Point3DFloats, one for each vertex, and one for a vector that points towards the center of the hull).
from C3DTEntity.m:
if (_hasTransform) {
glPushMatrix();
// Translation
if ((_translation.x != 0.0) || (_translation.y != 0.0) || (_translation.z != 0.0)) {
glTranslatef(_translation.x, _translation.y, _translation.z);
}
// Scaling
if ((_scaling.x != 1.0) || (_scaling.y != 1.0) || (_scaling.z != 1.0)) {
glScalef(_scaling.x, _scaling.y, _scaling.z);
}
// Rotation
glTranslatef(-_rotationCenter.x, -_rotationCenter.y, -_rotationCenter.z);
if (_rotation.w != 0.0) {
glRotatef(_rotation.w, _rotation.x, _rotation.y, _rotation.z);
} else {
if (_rotation.x != 0.0)
glRotatef(_rotation.x, 1.0f, 0.0f, 0.0f);
if (_rotation.y != 0.0)
glRotatef(_rotation.y, 0.0f, 1.0f, 0.0f);
if (_rotation.z != 0.0)
glRotatef(_rotation.z, 0.0f, 0.0f, 1.0f);
}
glTranslatef(_rotationCenter.x, _rotationCenter.y, _rotationCenter.z);
}
I added the bit in the above code that uses a single rotation around an axis (the "if (_rotation.w != 0.0)" bit), rather than a set of three rotations. My code is likely the problem, but I can't see how.
If your outvects don't all point in the correct directino, you might have to check your triangles' winding - are they all oriented the same way?
Additionally, it might be helpful to draw a line for each outvec (Use the average of the three vertices of your triangle as origin, and draw a line of a few units' length (depending on your scene's scale) into the direction of the outvect. This way, you can be sure that all your vectors are oriented correctly.
How do you calculate your outvects?
The problem appears to be in that glrotatef() expects degrees and I was giving it radians. In addition, clockwise rotation is taken to be positive, and so the sign of the rotation was wrong. This is the corrected code:
double rotAngle = -[outVect angleWith:unitZ]; // radians
[outVectEntity setRotationX: rotAxis.x
Y: rotAxis.y
Z: rotAxis.z
W: rotAngle * 180.0 / M_PI ];
I can now see that my other program has the inVects wrong (the outVects below are poking through the hull instead of pointing out from each face), and I can now track down that bug in the other program...tomorrow:

Fragment-shader blur ... how does this work?

uniform sampler2D sampler0;
uniform vec2 tc_offset[9];
void blur()
{
vec4 sample[9];
for(int i = 0; i < 9; ++i)
sample[i] = texture2D(sampler0, gl_TexCoord[0].st + tc_offset[i]);
gl_FragColor = (sample[0] + (2.0 * sample[1]) + sample[2] +
(2.0 * sample[3]) + sample[4] + 2.0 * sample[5] +
sample[6] + 2.0 * sample[7] + sample[8] ) / 13.0;
}
How does the sample[i] = texture2D(sample0, ...) line work?
It seems like to blur an image, I have to first generate the image, yet here, I'm somehow trying to query the very iamge I'm generating. How does this work?
It applies a blur kernel to the image. tc_offset needs to be properly initialized by the application to form a 3x3 area of sampling points around the actual texture coordinate:
0 0 0
0 x 0
0 0 0
(assuming x is the original coordinate). The offset for the upper-left sampling point would be -1/width,-1/height. The offset for the center point needs to be carefully aligned to texel center (the off-by-0.5 problem). Also, the hardware bilinear filter can be used to cheaply increase the amount of blur (by sampling between texels).
The rest of the shader scales the samples by their distance. Usually, this is precomputed as well:
for(int i = 0; i < NUM_SAMPLES; ++i) {
result += texture2D(sampler,texcoord+offsetscaling[i].xy)*offsetscaling[i].z;
}
One way is to generate your original image to render to a texture, not to the screen.
And then you draw a full screen quad using this shader and the texture as it's input to post-process the image.
As you note, in order to make a blurred image, you first need to make an image, and then blur it. This shader does (just) the second step, taking an image that was generated previously and blurring it. There needs to be additional code elsewhere to generate the original non-blurred image.

Smooth GPS data

I'm working with GPS data, getting values every second and displaying current position on a map. The problem is that sometimes (specially when accuracy is low) the values vary a lot, making the current position to "jump" between distant points in the map.
I was wondering about some easy enough method to avoid this. As a first idea, I thought about discarding values with accuracy beyond certain threshold, but I guess there are some other better ways to do. What's the usual way programs perform this?
Here's a simple Kalman filter that could be used for exactly this situation. It came from some work I did on Android devices.
General Kalman filter theory is all about estimates for vectors, with the accuracy of the estimates represented by covariance matrices. However, for estimating location on Android devices the general theory reduces to a very simple case. Android location providers give the location as a latitude and longitude, together with an accuracy which is specified as a single number measured in metres. This means that instead of a covariance matrix, the accuracy in the Kalman filter can be measured by a single number, even though the location in the Kalman filter is a measured by two numbers. Also the fact that the latitude, longitude and metres are effectively all different units can be ignored, because if you put scaling factors into the Kalman filter to convert them all into the same units, then those scaling factors end up cancelling out when converting the results back into the original units.
The code could be improved, because it assumes that the best estimate of current location is the last known location, and if someone is moving it should be possible to use Android's sensors to produce a better estimate. The code has a single free parameter Q, expressed in metres per second, which describes how quickly the accuracy decays in the absence of any new location estimates. A higher Q parameter means that the accuracy decays faster. Kalman filters generally work better when the accuracy decays a bit quicker than one might expect, so for walking around with an Android phone I find that Q=3 metres per second works fine, even though I generally walk slower than that. But if travelling in a fast car a much larger number should obviously be used.
public class KalmanLatLong {
private final float MinAccuracy = 1;
private float Q_metres_per_second;
private long TimeStamp_milliseconds;
private double lat;
private double lng;
private float variance; // P matrix. Negative means object uninitialised. NB: units irrelevant, as long as same units used throughout
public KalmanLatLong(float Q_metres_per_second) { this.Q_metres_per_second = Q_metres_per_second; variance = -1; }
public long get_TimeStamp() { return TimeStamp_milliseconds; }
public double get_lat() { return lat; }
public double get_lng() { return lng; }
public float get_accuracy() { return (float)Math.sqrt(variance); }
public void SetState(double lat, double lng, float accuracy, long TimeStamp_milliseconds) {
this.lat=lat; this.lng=lng; variance = accuracy * accuracy; this.TimeStamp_milliseconds=TimeStamp_milliseconds;
}
/// <summary>
/// Kalman filter processing for lattitude and longitude
/// </summary>
/// <param name="lat_measurement_degrees">new measurement of lattidude</param>
/// <param name="lng_measurement">new measurement of longitude</param>
/// <param name="accuracy">measurement of 1 standard deviation error in metres</param>
/// <param name="TimeStamp_milliseconds">time of measurement</param>
/// <returns>new state</returns>
public void Process(double lat_measurement, double lng_measurement, float accuracy, long TimeStamp_milliseconds) {
if (accuracy < MinAccuracy) accuracy = MinAccuracy;
if (variance < 0) {
// if variance < 0, object is unitialised, so initialise with current values
this.TimeStamp_milliseconds = TimeStamp_milliseconds;
lat=lat_measurement; lng = lng_measurement; variance = accuracy*accuracy;
} else {
// else apply Kalman filter methodology
long TimeInc_milliseconds = TimeStamp_milliseconds - this.TimeStamp_milliseconds;
if (TimeInc_milliseconds > 0) {
// time has moved on, so the uncertainty in the current position increases
variance += TimeInc_milliseconds * Q_metres_per_second * Q_metres_per_second / 1000;
this.TimeStamp_milliseconds = TimeStamp_milliseconds;
// TO DO: USE VELOCITY INFORMATION HERE TO GET A BETTER ESTIMATE OF CURRENT POSITION
}
// Kalman gain matrix K = Covarariance * Inverse(Covariance + MeasurementVariance)
// NB: because K is dimensionless, it doesn't matter that variance has different units to lat and lng
float K = variance / (variance + accuracy * accuracy);
// apply K
lat += K * (lat_measurement - lat);
lng += K * (lng_measurement - lng);
// new Covarariance matrix is (IdentityMatrix - K) * Covarariance
variance = (1 - K) * variance;
}
}
}
What you are looking for is called a Kalman Filter. It's frequently used to smooth navigational data. It is not necessarily trivial, and there is a lot of tuning you can do, but it is a very standard approach and works well. There is a KFilter library available which is a C++ implementation.
My next fallback would be least squares fit. A Kalman filter will smooth the data taking velocities into account, whereas a least squares fit approach will just use positional information. Still, it is definitely simpler to implement and understand. It looks like the GNU Scientific Library may have an implementation of this.
This might come a little late...
I wrote this KalmanLocationManager for Android, which wraps the two most common location providers, Network and GPS, kalman-filters the data, and delivers updates to a LocationListener (like the two 'real' providers).
I use it mostly to "interpolate" between readings - to receive updates (position predictions) every 100 millis for instance (instead of the maximum gps rate of one second), which gives me a better frame rate when animating my position.
Actually, it uses three kalman filters, on for each dimension: latitude, longitude and altitude.
They're independent, anyway.
This makes the matrix math much easier: instead of using one 6x6 state transition matrix, I use 3 different 2x2 matrices. Actually in the code, I don't use matrices at all. Solved all equations and all values are primitives (double).
The source code is working, and there's a demo activity.
Sorry for the lack of javadoc in some places, I'll catch up.
You should not calculate speed from position change per time. GPS may have inaccurate positions, but it has accurate speed (above 5km/h). So use the speed from GPS location stamp.
And further you should not do that with course, although it works most of the times.
GPS positions, as delivered, are already Kalman filtered, you probably cannot improve, in postprocessing usually you have not the same information like the GPS chip.
You can smooth it, but this also introduces errors.
Just make sure that your remove the positions when the device stands still, this removes jumping positions, that some devices/Configurations do not remove.
I usually use the accelerometers. A sudden change of position in a short period implies high acceleration. If this is not reflected in accelerometer telemetry it is almost certainly due to a change in the "best three" satellites used to compute position (to which I refer as GPS teleporting).
When an asset is at rest and hopping about due to GPS teleporting, if you progressively compute the centroid you are effectively intersecting a larger and larger set of shells, improving precision.
To do this when the asset is not at rest you must estimate its likely next position and orientation based on speed, heading and linear and rotational (if you have gyros) acceleration data. This is more or less what the famous K filter does. You can get the whole thing in hardware for about $150 on an AHRS containing everything but the GPS module, and with a jack to connect one. It has its own CPU and Kalman filtering on board; the results are stable and quite good. Inertial guidance is highly resistant to jitter but drifts with time. GPS is prone to jitter but does not drift with time, they were practically made to compensate each other.
One method that uses less math/theory is to sample 2, 5, 7, or 10 data points at a time and determine those which are outliers. A less accurate measure of an outlier than a Kalman Filter is to to use the following algorithm to take all pair wise distances between points and throw out the one that is furthest from the the others. Typically those values are replaced with the value closest to the outlying value you are replacing
For example
Smoothing at five sample points A, B, C, D, E
ATOTAL = SUM of distances AB AC AD AE
BTOTAL = SUM of distances AB BC BD BE
CTOTAL = SUM of distances AC BC CD CE
DTOTAL = SUM of distances DA DB DC DE
ETOTAL = SUM of distances EA EB EC DE
If BTOTAL is largest you would replace point B with D if BD = min { AB, BC, BD, BE }
This smoothing determines outliers and can be augmented by using the midpoint of BD instead of point D to smooth the positional line. Your mileage may vary and more mathematically rigorous solutions exist.
As for least squares fit, here are a couple other things to experiment with:
Just because it's least squares fit doesn't mean that it has to be linear. You can least-squares-fit a quadratic curve to the data, then this would fit a scenario in which the user is accelerating. (Note that by least squares fit I mean using the coordinates as the dependent variable and time as the independent variable.)
You could also try weighting the data points based on reported accuracy. When the accuracy is low weight those data points lower.
Another thing you might want to try is rather than display a single point, if the accuracy is low display a circle or something indicating the range in which the user could be based on the reported accuracy. (This is what the iPhone's built-in Google Maps application does.)
Going back to the Kalman Filters ... I found a C implementation for a Kalman filter for GPS data here: http://github.com/lacker/ikalman I haven't tried it out yet, but it seems promising.
You can also use a spline. Feed in the values you have and interpolate points between your known points. Linking this with a least-squares fit, moving average or kalman filter (as mentioned in other answers) gives you the ability to calculate the points inbetween your "known" points.
Being able to interpolate the values between your knowns gives you a nice smooth transition and a /reasonable/ approximation of what data would be present if you had a higher-fidelity. http://en.wikipedia.org/wiki/Spline_interpolation
Different splines have different characteristics. The one's I've seen most commonly used are Akima and Cubic splines.
Another algorithm to consider is the Ramer-Douglas-Peucker line simplification algorithm, it is quite commonly used in the simplification of GPS data. (http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm)
Here's a Javascript implementation of #Stochastically's Java implementation for anyone needing it:
class GPSKalmanFilter {
constructor (decay = 3) {
this.decay = decay
this.variance = -1
this.minAccuracy = 1
}
process (lat, lng, accuracy, timestampInMs) {
if (accuracy < this.minAccuracy) accuracy = this.minAccuracy
if (this.variance < 0) {
this.timestampInMs = timestampInMs
this.lat = lat
this.lng = lng
this.variance = accuracy * accuracy
} else {
const timeIncMs = timestampInMs - this.timestampInMs
if (timeIncMs > 0) {
this.variance += (timeIncMs * this.decay * this.decay) / 1000
this.timestampInMs = timestampInMs
}
const _k = this.variance / (this.variance + (accuracy * accuracy))
this.lat += _k * (lat - this.lat)
this.lng += _k * (lng - this.lng)
this.variance = (1 - _k) * this.variance
}
return [this.lng, this.lat]
}
}
Usage example:
const kalmanFilter = new GPSKalmanFilter()
const updatedCoords = []
for (let index = 0; index < coords.length; index++) {
const { lat, lng, accuracy, timestampInMs } = coords[index]
updatedCoords[index] = kalmanFilter.process(lat, lng, accuracy, timestampInMs)
}
Mapped to CoffeeScript if anyones interested. **edit -> sorry using backbone too, but you get the idea.
Modified slightly to accept a beacon with attribs
{latitude: item.lat,longitude: item.lng,date: new
Date(item.effective_at),accuracy: item.gps_accuracy}
MIN_ACCURACY = 1
# mapped from http://stackoverflow.com/questions/1134579/smooth-gps-data
class v.Map.BeaconFilter
constructor: ->
_.extend(this, Backbone.Events)
process: (decay,beacon) ->
accuracy = Math.max beacon.accuracy, MIN_ACCURACY
unless #variance?
# if variance nil, inititalise some values
#variance = accuracy * accuracy
#timestamp_ms = beacon.date.getTime();
#lat = beacon.latitude
#lng = beacon.longitude
else
#timestamp_ms = beacon.date.getTime() - #timestamp_ms
if #timestamp_ms > 0
# time has moved on, so the uncertainty in the current position increases
#variance += #timestamp_ms * decay * decay / 1000;
#timestamp_ms = beacon.date.getTime();
# Kalman gain matrix K = Covarariance * Inverse(Covariance + MeasurementVariance)
# NB: because K is dimensionless, it doesn't matter that variance has different units to lat and lng
_k = #variance / (#variance + accuracy * accuracy)
#lat = _k * (beacon.latitude - #lat)
#lng = _k * (beacon.longitude - #lng)
#variance = (1 - _k) * #variance
[#lat,#lng]
I have transformed the Java code from #Stochastically to Kotlin
class KalmanLatLong
{
private val MinAccuracy: Float = 1f
private var Q_metres_per_second: Float = 0f
private var TimeStamp_milliseconds: Long = 0
private var lat: Double = 0.toDouble()
private var lng: Double = 0.toDouble()
private var variance: Float =
0.toFloat() // P matrix. Negative means object uninitialised. NB: units irrelevant, as long as same units used throughout
fun KalmanLatLong(Q_metres_per_second: Float)
{
this.Q_metres_per_second = Q_metres_per_second
variance = -1f
}
fun get_TimeStamp(): Long { return TimeStamp_milliseconds }
fun get_lat(): Double { return lat }
fun get_lng(): Double { return lng }
fun get_accuracy(): Float { return Math.sqrt(variance.toDouble()).toFloat() }
fun SetState(lat: Double, lng: Double, accuracy: Float, TimeStamp_milliseconds: Long)
{
this.lat = lat
this.lng = lng
variance = accuracy * accuracy
this.TimeStamp_milliseconds = TimeStamp_milliseconds
}
/// <summary>
/// Kalman filter processing for lattitude and longitude
/// https://stackoverflow.com/questions/1134579/smooth-gps-data/15657798#15657798
/// </summary>
/// <param name="lat_measurement_degrees">new measurement of lattidude</param>
/// <param name="lng_measurement">new measurement of longitude</param>
/// <param name="accuracy">measurement of 1 standard deviation error in metres</param>
/// <param name="TimeStamp_milliseconds">time of measurement</param>
/// <returns>new state</returns>
fun Process(lat_measurement: Double, lng_measurement: Double, accuracy: Float, TimeStamp_milliseconds: Long)
{
var accuracy = accuracy
if (accuracy < MinAccuracy) accuracy = MinAccuracy
if (variance < 0)
{
// if variance < 0, object is unitialised, so initialise with current values
this.TimeStamp_milliseconds = TimeStamp_milliseconds
lat = lat_measurement
lng = lng_measurement
variance = accuracy * accuracy
}
else
{
// else apply Kalman filter methodology
val TimeInc_milliseconds = TimeStamp_milliseconds - this.TimeStamp_milliseconds
if (TimeInc_milliseconds > 0)
{
// time has moved on, so the uncertainty in the current position increases
variance += TimeInc_milliseconds.toFloat() * Q_metres_per_second * Q_metres_per_second / 1000
this.TimeStamp_milliseconds = TimeStamp_milliseconds
// TO DO: USE VELOCITY INFORMATION HERE TO GET A BETTER ESTIMATE OF CURRENT POSITION
}
// Kalman gain matrix K = Covarariance * Inverse(Covariance + MeasurementVariance)
// NB: because K is dimensionless, it doesn't matter that variance has different units to lat and lng
val K = variance / (variance + accuracy * accuracy)
// apply K
lat += K * (lat_measurement - lat)
lng += K * (lng_measurement - lng)
// new Covarariance matrix is (IdentityMatrix - K) * Covarariance
variance = (1 - K) * variance
}
}
}