Đây là một bộ lọc Kalman đơn giản có thể được sử dụng cho chính xác tình huống này. Nó đến từ một số công việc tôi đã làm trên các thiết bị Android.
Lý thuyết bộ lọc Kalman chung là tất cả về các ước tính cho vectơ, với độ chính xác của các ước tính được biểu thị bằng ma trận hiệp phương sai. Tuy nhiên, để ước tính vị trí trên các thiết bị Android, lý thuyết chung giảm xuống một trường hợp rất đơn giản. Các nhà cung cấp vị trí Android cung cấp vị trí dưới dạng vĩ độ và kinh độ, cùng với độ chính xác được chỉ định là một số duy nhất được đo bằng mét. Điều này có nghĩa là thay vì ma trận hiệp phương sai, độ chính xác trong bộ lọc Kalman có thể được đo bằng một số duy nhất, mặc dù vị trí trong bộ lọc Kalman được đo bằng hai số. Ngoài ra, thực tế là vĩ độ, kinh độ và mét có thể bị bỏ qua một cách hiệu quả, bởi vì nếu bạn đặt các yếu tố tỷ lệ vào bộ lọc Kalman để chuyển đổi tất cả chúng thành cùng một đơn vị,
Mã có thể được cải thiện, vì nó giả định rằng ước tính tốt nhất về vị trí hiện tại là vị trí được biết đến cuối cùng và nếu ai đó đang di chuyển, có thể sử dụng cảm biến của Android để đưa ra ước tính tốt hơn. Mã này có một tham số miễn phí duy nhất Q, được biểu thị bằng mét trên giây, mô tả mức độ chính xác phân rã nhanh chóng khi không có bất kỳ ước tính vị trí mới nào. Tham số Q cao hơn có nghĩa là độ chính xác phân rã nhanh hơn. Các bộ lọc Kalman thường hoạt động tốt hơn khi độ chính xác phân rã nhanh hơn một chút so với mong đợi, vì vậy để đi bộ với điện thoại Android tôi thấy rằng Q = 3 mét mỗi giây hoạt động tốt, mặc dù tôi thường đi chậm hơn thế. Nhưng nếu đi trong một chiếc xe nhanh, một số lượng lớn hơn nhiều rõ ràng nên được sử dụng.
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;
}
}
}