Kalman Filter for CLLocation

KalmanFilter.swift

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)
    }

}

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?