Kalman, least squares, or

Question

In an effort to help people understand what i the question is that i am asking, i have chosen to reword it entirely. I hope this clears it up.

i am collecting gps data (lat/long) at a 1 second rate. With understanding that this data may not be 100% accurate, and have an occasional (1 or more) datapoint that is far off the mark, what would be the most appropriate method to remove the outlier points in order to determine a fairly accurate course and speed of a vehicle? This vehicle can travel anywhere from 0-60 miles an hour, generally in straight lines, but also can be prone to sudden turns (weighted values?).

I apologize for the confusion, and moreso for failing to understand the suggestions already handed out.

Answer 1

the problem appears to be ill-formed because you don't have enough data. So, your GPS is collecting positions, these are basically a bunch of coordinates. And you are asking if "they are correct" and "how to make it more precise". Clearly, we need more data to do that.

In robotics, the typical "another data" is data from other sensors (such as IMU - inertial measurement unit, practically an accelerometer) or odometry (commands to the motors). From both of these, the robot knows it is "going straight" and any offshoot to the left / right can be corrected. Alternatively, they track "landmarks" (such as trees or corners) using computer vision algorithms, which also provides a good information about the movement of the robot. You don't have any of those.

What you do have, is the physical model of the car. You know that at 60 MPH the car can't possibly make 90 degree turns (that's why the problem doesn't seem ill-suited to you, because you naturally know how the car should behave). This constraint is not nearly as good as additional sensor information would be, but it should do. You can use Nonlinear Least Squares or Kalman Filter alike.

I'm not a big fan of Kalman filters so I won't tell you how to implement that.

With NLS, you have the position of the car as a graph (not to be confused with a plot). Each position of the car is a vertex. Every two adjacent vertices (corresponding to "the previous and the current position") are linked by the "laws of car motion" constraints (edges). Each vertex also has the GPS location constraint, which is an unary edge.

This kind of graph is represented by a sparse matrix. It is a Jacobian matrix (or Hessian matrix), where the values correspond to derivations of the vertices with respect to the constraints at the given system state (position of all the vertices). At each step the matrix size grows because the next position is added. To maintain low complexity of the solution, you can drop the old positions, keeping only the last N steps. At each step you need to evaluate the physics constraints (calculate speed and rate of rotation and see if it is plausible) and calculate the Jacobians / Hessians and an error vector (vector of differences of current vector positions from the GPS fixes / from physically constrained motion). Then you solve this system (dx = Jacobians / errors), yielding the vector dx, which is difference in vertex positions. You just add it to the vertices and you're there. That is essentially the Gauss-Newton algorithm.

This is not an easy thing to implement. There are libraries that solve these kind of graph problems efficiently, such as SLAM++, iSAM, GTSAM or g2o. The trouble is, none of these will work for you out-of-the-box, as the physical plausibility constraint is not implemented there (nor is the GPS constraint, but that one is just subtraction). You will have to implement your own vertex / edge types.

I suggest you use something simpler. Just take differences of what the GPS tells you, calculate windowed median and see if the last measurement is too far off median. In case it is too far (you will have to experiment and see which threshold works), do not use that measurement for speed / course calculation (but still keep it for calculating the medians). That should be fairly accurate and ok for your purposes.

If you upload your measured data as a text file with lat / lon / timestamp of you driving arround the block, we can see about writing the code to process that.

Answer 2

The standard algorithm adjacent_difference will produce the difference between each element in a range of iterators. So If there are 5 elements it produces 4 differences.

These are the standard libraries we'll be using:

#include <vector>
#include <iostream>
#include <iterator>
#include <algorithm>
#include <numeric>

I don't know what your GPS class would look like. I'm going to assume it's one dimensional:

class Position
{
public:
    Position() :
    m_position(0)
    {
    }

    Position(int position) :
    m_position(position)
    {
    }

    Position operator-(const Position& other) const
    {
        return Position(m_position - other.m_position);
    }
    operator int() const
    {
        return m_position;
    }
private:
    int m_position;
};

Position abs_sum(const Position& lhs, const Position& rhs)
{
    return Position(abs(int(lhs)) + abs(int(rhs)));
}

Putting it together:

int main()
{
    using namespace std; // for brevity - don't really do this in your code

    vector<Position> positions;
    positions.push_back(Position(13));
    positions.push_back(Position(23));
    positions.push_back(Position(17));
    positions.push_back(Position(19));

    vector<Position> displacements;

    adjacent_difference(positions.begin(), positions.end(),
                        back_inserter(displacements));

    cout << "Displacements: ";
    copy(displacements.begin(), displacements.end(),
         ostream_iterator<int>(cout, ", "));

    cout << endl;

    int distance = accumulate(displacements.begin(), displacements.end(),
                              0, abs_sum);
    cout << "Total: " << distance << endl;

    return 0;
}

Output:

Displacements: 13, 10, -6, 2, 
Total: 31