determinate: is point on line segment - line

I am trying to code a java methods which returns a Boolean true if a point(x,y) is on a line segment and false if not.
I tried this:
public static boolean OnDistance(MyLocation a, MyLocation b, MyLocation queryPoint) {
double value = java.lang.Math.signum((a.mLongitude - b.mLongitude) * (queryPoint.mLatitude - a.mLatitude)
- (b.mLatitude - a.mLatitude) * (queryPoint.mLongitude - a.mLongitude));
double compare = 1;
if (value == compare) {
return true;
}
return false;
}
but it doesn't work.

I am not JAVA coder so I stick to math behind ... For starters let assume you are on plane (not sphere surface)
I would use Vector math so let:
a,b - be the line endpoints
q - queried point
c=q-a - queried line direction vector
d=b-a - line direction vector
use dot product for parameter extraction
t=dot(c,d)/(|c|*|d|)
t is line parameter <0,1> if out of range q is not inside line
|c|=sqrt(c.x*c.x+c.y*c.y) size of vector
dot(c,d)=c.x*d.x+c.y*d.y scalar vector multiply
now compute corresponding point on line
e=a+(t*d)
e is the closest point to q on the line ab
compute perpendicular distance of q and ab
l=|q-e|;
if (l>treshold) then q is not on line ab else it is on the line ab. The threshold is the max distance from line you are still accepting as inside line. No need to have l sqrt-ed the threshold constant can be powered by 2 instead for speed.
if you add all this to single equation
then some things will simplify itself (hope did not make some silly math mistake)
l=|(q-a)-(b-a)*(dot(q-a,b-a)/|b-a|^2)|;
return (l<=treshold);
or
l=|c-(d*dot(c,d)/|d|^2)|;
return (l<=treshold);
As you can see we do not even need sqrt for this :)
[Notes]
If you need spherical or ellipsoidal surface instead then you need to specify it closer which it is what are the semi axises. The line become arc/curve and need some corrections which depends on the shape of surface see
Projecting a point onto a path
but can be done also by approximation and may be also by binary search of point e see:
mine approx class in C++
The vector math used can be found here at the end:
Understanding 4x4 homogenous transform matrices
Here 3D C++ implementation (with different names):
double distance_point_axis(double *p,double *p0,double *dp)
{
int i;
double l,d,q[3];
for (i=0;i<3;i++) q[i]=p[i]-p0[i]; // q = p-p0
for (l=0.0,i=0;i<3;i++) l+=dp[i]*dp[i]; // l = |dp|^2
for (d=0.0,i=0;i<3;i++) d+=q[i]*dp[i]; // d = dot(q,dp)
if (l<1e-10) d=0.0; else d/=l; // d = dot(q,dp)/|dp|^2
for (i=0;i<3;i++) q[i]-=dp[i]*d; // q=q-dp*dot(q,dp)/|dp|^2
for (l=0.0,i=0;i<3;i++) l+=q[i]*q[i]; l=sqrt(l); // l = |q|
return l;
}
Where p0[3] is any point on axis and dp[3] is direction vector of axis. The p[3] is the queried point you want the distance to axis for.

Related

Shortest rotation between two vectors not working like expected

def signed_angle_between_vecs(target_vec, start_vec, plane_normal=None):
start_vec = np.array(start_vec)
target_vec = np.array(target_vec)
start_vec = start_vec/np.linalg.norm(start_vec)
target_vec = target_vec/np.linalg.norm(target_vec)
if plane_normal is None:
arg1 = np.dot(np.cross(start_vec, target_vec), np.cross(start_vec, target_vec))
else:
arg1 = np.dot(np.cross(start_vec, target_vec), plane_normal)
arg2 = np.dot(start_vec, target_vec)
return np.arctan2(arg1, arg2)
from scipy.spatial.transform import Rotation as R
world_frame_axis = input_rotation_object.apply(canonical_axis)
angle = signed_angle_between_vecs(canonical_axis, world_frame_axis)
axis_angle = np.cross(world_frame_axis, canonical_axis) * angle
C = R.from_rotvec(axis_angle)
transformed_world_frame_axis_to_canonical = C.apply(world_frame_axis)
I am trying to align world_frame_axis to canonical_axis by performing a rotation around the normal vector generated by the cross product between the two vectors, using the signed angle between the two axes.
However, this code does not work. If you start with some arbitrary rotation as input_rotation_object you will see that transformed_world_frame_axis_to_canonical does not match canonical_axis.
What am I doing wrong?
not a python coder so I might be wrong but this looks suspicious:
start_vec = start_vec/np.linalg.norm(start_vec)
from the names I would expect that np.linalg.norm normalizes the vector already so the line should be:
start_vec = np.linalg.norm(start_vec)
and all the similar lines too ...
Also the atan2 operands are not looking right to me. I would (using math):
a = start_vec / |start_vec | // normalized start
b = target_vec / |target_vec| // normalized end
u = a // normalized one axis of plane
v = cross(u ,b)
v = cross(v ,u)
v = v / |v| // normalized second axis of plane perpendicular to u
dx = dot(u,b) // target vector in 2D aligned to start
dy = dot(v,b)
ang = atan2(dy,dx)
beware the ang might negated (depending on your notations) if the case either add minus sign or reverse the order in cross(u,v) to cross(v,u) Also you can do sanity check with comparing result to unsigned:
ang' = acos(dot(a,b))
in absolute values they should be the same (+/- rounding error).

Switch on argument type

Using Open SCAD, I have a module that, like cube(), has a size parameter that can be a single value or a vector of three values. Ultimately, I want a vector of three values.
If the caller passes a single value, I'd like all three values of the vector to be the same. I don't see anything in the language documentation about detecting the type of an argument. So I came up with this hack:
module my_cubelike_thing(size=1) {
dimensions = concat(size, size, size);
width = dimensions[0];
length = dimensions[1];
height = dimensions[2];
// ... use width, length, and height ...
}
When size is a single value, the result of the concat is exactly what I want: three copies of the value.
When size is a three-value vector, the result of the concat is nine-value vector, and my code just ignores the last six values.
It works but only because what I want in the single value case is to replicate the value. Is there a general way to switch on the argument type and do different things depending on that type?
If type of size only can be single value or a vector with 3 values, the type can helpwise be found by the special value undef:
a = [3,5,8];
// a = 5;
if (a[0] == undef) {
dimensions = concat(a, a, a);
// do something
cube(size=dimensions,center=false);
}
else {
dimensions = a;
// do something
cube(size=dimensions,center=false);
}
But assignments are only valid in the scope in which they are defined , documnetation of openscad.
So in each subtree much code is needed and i would prefere to validate the type of size in an external script (e.g. python3) and write the openscad-code with the assignment of variables to a file, which can be included in the openscad-file, here my short test-code:
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import os
# size = 20
size = [20,15,10]
if type(size) == int:
dimensions = [size, size, size]
elif type(size) == list:
dimensions = size
else:
# if other types possible
pass
with open('variablen.scad', 'w') as wObj:
for i, v in enumerate(['l', 'w', 'h']):
wObj.write('{} = {};\n'.format(v, dimensions[i]))
os.system('openscad ./typeDef.scad')
content of variablen.scad:
l = 20;
w = 15;
h = 10;
and typeDef.scad can look like this
include <./variablen.scad>;
module my_cubelike_thing() {
linear_extrude(height=h, center=false) square(l, w);
}
my_cubelike_thing();

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.

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
}
}
}