我正在处理 GPS 数据,每秒获取值并在地图上显示当前位置。问题是有时(特别是当精度较低时)值变化很大,使得当前位置在地图中的远点之间“跳跃”。
我想知道一些足够简单的方法来避免这种情况。作为第一个想法,我考虑过以超过某个阈值的精度丢弃值,但我想还有其他更好的方法可以做。程序执行此操作的通常方式是什么?
这是一个简单的卡尔曼滤波器,可以用于这种情况。它来自我在 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;
}
}
}
这可能来得有点晚……
我为 Android 编写了这个KalmanLocationManager,它包装了两个最常见的位置提供程序,网络和 GPS,卡尔曼过滤数据,并将更新传递给LocationListener
(如两个“真实”提供程序)。
我主要使用它在读数之间“插值” - 例如每 100 毫秒接收一次更新(位置预测)(而不是一秒的最大 gps 速率),这在为我的位置设置动画时给了我更好的帧速率。
实际上,它使用三个卡尔曼滤波器,每个维度:纬度、经度和高度。反正他们是独立的。
这使得矩阵数学变得更加容易:我没有使用一个 6x6 状态转换矩阵,而是使用了 3 个不同的 2x2 矩阵。实际上在代码中,我根本不使用矩阵。求解了所有方程,所有值都是基元(双精度)。
源代码正在运行,并且有一个演示活动。抱歉有些地方没有javadoc,我会赶上的。
您不应该根据每次的位置变化来计算速度。GPS可能有不准确的位置,但它有准确的速度(5公里/小时以上)。所以使用来自 GPS 位置标记的速度。此外,您不应该在课程中这样做,尽管它在大多数情况下都有效。
交付时的 GPS 位置已经经过卡尔曼滤波,您可能无法改进,在后处理中通常您没有与 GPS 芯片相同的信息。
您可以对其进行平滑处理,但这也会引入错误。
只需确保在设备静止时删除位置,这会删除跳跃位置,某些设备/配置不会删除。
我通常使用加速度计。在短时间内突然改变位置意味着高加速度。如果这没有反映在加速度计遥测中,那几乎可以肯定是由于用于计算位置的“最佳三颗”卫星发生了变化(我称之为 GPS 传送)。
当资产由于 GPS 传送而处于静止状态和跳跃状态时,如果您逐步计算质心,您将有效地与越来越大的外壳相交,从而提高精度。
要在资产不静止时执行此操作,您必须根据速度、航向以及线性和旋转(如果您有陀螺仪)加速度数据估计其可能的下一个位置和方向。这或多或少是著名的 K 滤波器所做的。您可以在 AHRS 上以大约 150 美元的价格获得整个硬件,该 AHRS 包含除 GPS 模块之外的所有东西,并带有一个用于连接的插孔。它有自己的 CPU 和板载卡尔曼滤波;结果稳定且相当好。惯性制导对抖动有很强的抵抗力,但会随时间漂移。GPS容易抖动,但不会随时间漂移,它们实际上是为了相互补偿。
一种使用较少数学/理论的方法是一次采样 2、5、7 或 10 个数据点,并确定那些异常值。比卡尔曼滤波器更不准确的异常值测量是使用以下算法来获取点之间的所有成对距离,并丢弃离其他点最远的一个。通常,这些值将替换为最接近您要替换的异常值的值
例如
在五个采样点 A、B、C、D、E 处进行平滑处理
ATOTAL = AB AC AD AE 距离的总和
BTOTAL = 距离之和 AB BC BD BE
CTOTAL = 距离之和 AC BC CD CE
DTOTAL = 距离之和 DA DB DC DE
ETOTAL = 距离之和 EA EB EC DE
如果 BTOTAL 最大,如果 BD = min { AB, BC, BD, BE },您将用 D 替换点 B
这种平滑确定了异常值,并且可以通过使用 BD 的中点而不是点 D 来平滑位置线来增强。您的里程可能会有所不同,并且存在更严格的数学解决方案。
至于最小二乘拟合,这里还有一些其他的东西可以尝试:
仅仅因为它是最小二乘拟合并不意味着它必须是线性的。您可以对数据进行二次曲线的最小二乘拟合,然后这将适合用户正在加速的场景。(请注意,最小二乘拟合是指使用坐标作为因变量,时间作为自变量。)
您还可以尝试根据报告的准确性对数据点进行加权。当准确性较低时,这些数据点的权重较低。
您可能想要尝试的另一件事是,而不是显示单个点,如果准确度较低,则显示一个圆圈或指示用户可以基于报告的准确度的范围的东西。(这就是 iPhone 内置的谷歌地图应用程序所做的。)
回到卡尔曼滤波器......我在这里找到了一个用于 GPS 数据的卡尔曼滤波器的 C 实现:http: //github.com/lacker/ikalman我还没有尝试过,但它似乎很有希望。
您也可以使用样条曲线。输入您拥有的值并在已知点之间插入点。将其与最小二乘拟合、移动平均线或卡尔曼滤波器(如其他答案中所述)联系起来,使您能够计算“已知”点之间的点。
能够在您的已知值之间插入值,可以为您提供一个很好的平滑过渡和 /reasonable / 近似值,如果你有更高的保真度,将会出现什么数据。http://en.wikipedia.org/wiki/Spline_interpolation
不同的样条具有不同的特性。我见过最常用的是 Akima 和 Cubic splines。
另一个需要考虑的算法是 Ramer-Douglas-Peucker 线简化算法,它在 GPS 数据的简化中非常常用。(http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm)
这是 @Stochastically 的 Java 实现的 Javascript 实现,供任何需要它的人使用:
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]
}
}
使用示例:
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)
}
如果有人感兴趣,映射到 CoffeeScript。**edit -> 很抱歉也使用了主干,但你明白了。
稍作修改以接受带有属性的信标
{纬度:item.lat,经度:item.lng,日期:新日期(item.effective_at),精度: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]
我已将 Java 代码从 @Stochasticly 转换为 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
}
}
}