这是一个简单的卡尔曼滤波器，可以用于这种情况。它来自我在Android设备上所做的一些工作。
一般的卡尔曼滤波理论都是关于向量的估计，其估计的准确性由协方差矩阵表示。但是，为了估计Android设备上的位置，一般理论简化为一个非常简单的情况。Android位置提供程序将位置表示为纬度和经度，以及指定为以米为单位的单个数字的精度。这意味着，可以用一个数字来测量卡尔曼滤波器中的精度，而不用协方差矩阵，即使卡尔曼滤波器中的位置可以用两个数字来衡量。同样，纬度，经度和米实际上都是所有不同单位的事实也可以忽略不计，因为如果您将比例因子放入卡尔曼滤波器中，以将它们全部转换为相同单位，
该代码可以进行改进，因为它假定当前位置的最佳估计是最后一个已知位置，并且如果有人在移动，则应该可以使用Android的传感器来产生更好的估计。该代码具有单个自由参数Q，以米/秒表示，描述了在没有任何新位置估计的情况下精度下降的速度。Q参数越高，意味着精度下降得越快。当精度下降的速度比人们预期的快一点时，卡尔曼滤波器通常会更好地工作，因此，对于Android手机，我发现Q = 3米/秒的效果很好，尽管我通常走得慢一些。但是，如果乘坐快车旅行，显然应该使用更大的数量。
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;
}
}
}