Skip to content

Instantly share code, notes, and snippets.

@ShawnMcCool
Forked from daniellemccool/KalmanFilter.cs
Created September 10, 2018 12:13
Show Gist options
  • Save ShawnMcCool/2ae5bf6dfdf1a2ee962cdcef83669242 to your computer and use it in GitHub Desktop.
Save ShawnMcCool/2ae5bf6dfdf1a2ee962cdcef83669242 to your computer and use it in GitHub Desktop.
KalmanFilter
using System;
using System.Collections.Generic;
using System.Text;
using System.Linq;
using Tabi.DataObjects;
namespace Tabi.Logic
{
public class KalmanLatLong : PositionEntry // Implementation taken from: https://stackoverflow.com/questions/1134579/smooth-gps-data/15657798#15657798
{
//private final float MinAccuracy = 1;
private 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 GetTimeStamp()
{
return TimeStamp_milliseconds;
}
public double GetLat()
{
return lat;
}
public double GetLng()
{
return lng;
}
public float GetAccuracy()
{
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;
}
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment