Skip to content

Instantly share code, notes, and snippets.

@trevphil
Created June 3, 2019 13:24
Show Gist options
  • Save trevphil/8762152832c5fdbadd323ef03406053f to your computer and use it in GitHub Desktop.
Save trevphil/8762152832c5fdbadd323ef03406053f to your computer and use it in GitHub Desktop.
Kalman Filter for CLLocation
import Foundation
import CoreLocation
extension CLLocation {
// Alias for `horizontalAccuracy` (more readable)
var uncertainty: Double {
return horizontalAccuracy
}
}
class KalmanFilter {
// MARK: - Private Properties
/// Small term used to avoid dividing by zero
private let epsilon: Double = 1e-5
/// A parameter for how much uncertainty we want to introduce between measurements. It has units in
/// meters per sec, for how much we believe the user may have traveled in one second. A larger
/// value means we think the user traveled further, therefore we will rely less on the previous estimate.
var timeUncertainty: Double
/// Internal location estimate used to perform filter computations
private(set) var locationEstimate: CLLocation?
/// Covariance matrix (one-dimensional) used in computations
private var variance: Double = 0
/// Standard deviation (i.e. uncertainty) derived from covariance
private var uncertainty: Double {
return sqrt(variance)
}
// MARK: - Initialization
init(timeUncertainty: Double = 0) {
self.timeUncertainty = timeUncertainty
}
// MARK: - Public Functions
/// Run a Kalman Filter iteration to estimate a new location. First, the uncertainty of the current estimate is
/// updated based on the elapsed time and the predicted speed of the tracked object. If an accurate measurement
/// is available, the prediction is then adjusted taking into consideration the accuracy of the measurement.
///
/// - Parameter measurement: A new location measurement
/// - Returns: The new estimated location
func process(measurement: CLLocation) -> CLLocation {
var updatedEstimate = locationEstimate == nil ?
initialEstimate(using: measurement) :
predict(using: measurement.timestamp)
// If the measurement is accurate enough, that we will use it to derive a more accurate updated estimate
if measurement.uncertainty >= 0, let estimate = locationEstimate {
updatedEstimate = correct(measurement: measurement, estimate: estimate)
}
locationEstimate = updatedEstimate
return updatedEstimate
}
// MARK: - Private Functions
private func initialEstimate(using measurement: CLLocation) -> CLLocation {
variance = pow(measurement.uncertainty, 2)
return measurement
}
private func predict(using time: Date) -> CLLocation {
guard let estimate = locationEstimate else {
return CLLocation()
}
let timeDelta = time.timeIntervalSince1970 - estimate.timestamp.timeIntervalSince1970
if timeDelta > 0 {
// Time has moved on, so the uncertainty in the current position increases
variance += timeDelta * pow(timeUncertainty, 2)
}
return estimate.location(withUncertainty: uncertainty).location(withTimestamp: time)
}
private func correct(measurement: CLLocation, estimate: CLLocation) -> CLLocation {
// Kalman gain matrix gainMatrix = Covariance / (Covariance + MeasurementVariance)
let gain = variance / (variance + pow(measurement.uncertainty, 2) + epsilon)
// New covariance matrix is (IdentityMatrix - GainMatrix) * Covariance
variance = (1 - gain) * variance
// Apply the gain matrix
let newLat = estimate.latitude + gain * (measurement.latitude - estimate.latitude)
let newLong = estimate.longitude + gain * (measurement.longitude - estimate.longitude)
let newCoord = CLLocationCoordinate2D(latitude: newLat, longitude: newLong)
let newAlt = estimate.altitude + gain * (measurement.altitude - estimate.altitude)
return estimate
.location(withUncertainty: uncertainty)
.location(withCoordinate: newCoord)
.location(withAltitude: newAlt)
.location(withTimestamp: measurement.timestamp)
}
}
@jeffbailey
Copy link

Thanks for writing the article! I see this code does not compile. For example, it look like a extension for CLLocation is missing for the location(withUncertainly:) method. Can you update?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment