ปรับข้อมูล GPS ให้ราบรื่น

ฉันกำลังทำงานกับข้อมูล 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;
        }
    }
}

สิ่งที่คุณกำลังมองหาที่เรียกว่ากรองคาลมาน มักใช้เพื่อทำให้ข้อมูลการเดินเรือราบรื่นขึ้น ไม่จำเป็นต้องมีเรื่องเล็กน้อยและมีการปรับแต่งมากมายที่คุณสามารถทำได้ แต่มันเป็นวิธีการมาตรฐานและทำงานได้ดี มีไลบรารี KFilterอยู่ซึ่งเป็นการนำ C ++ ไปใช้งาน

ทางเลือกต่อไปของฉันจะเป็นสี่เหลี่ยมน้อยพอดี ตัวกรองคาลมานจะทำให้ข้อมูลโดยคำนึงถึงความเร็วในขณะที่วิธีการกำลังสองน้อยที่สุดจะใช้ข้อมูลตำแหน่ง ถึงกระนั้นก็เป็นเรื่องง่ายที่จะใช้และเข้าใจ ดูเหมือนว่าห้องสมุดวิทยาศาสตร์ของ GNU อาจมีการใช้สิ่งนี้

นี่อาจจะมาสาย ...

ฉันเขียนKalmanLocationManagerนี้สำหรับ Android ซึ่งครอบคลุมผู้ให้บริการตำแหน่งที่พบมากที่สุดสองรายคือเครือข่ายและจีพีเอสคาลมานกรองข้อมูลและมอบการอัปเดตให้กับLocationListener(เช่นผู้ให้บริการสองราย)

ฉันใช้ส่วนใหญ่ในการ "สอดแทรก" ระหว่างการอ่าน - เพื่อรับการอัปเดต (การคาดการณ์ตำแหน่ง) ทุก ๆ 100 มิลลิวินาที (แทนอัตรา gps สูงสุดหนึ่งวินาที) ซึ่งทำให้ฉันมีอัตราเฟรมที่ดีขึ้นเมื่อเคลื่อนไหวตำแหน่งของฉัน

ที่จริงแล้วมันใช้ตัวกรองคาลมานสามตัวสำหรับแต่ละส่วนข้อมูล: ละติจูดลองจิจูดและความสูง พวกเขาเป็นอิสระอย่างไรก็ตาม

สิ่งนี้ทำให้คณิตศาสตร์เมทริกซ์ง่ายขึ้นมากแทนที่จะใช้เมทริกซ์การเปลี่ยนสถานะ 6x6 หนึ่งครั้งฉันใช้เมทริกซ์ 2x2 ที่แตกต่างกัน 3 ตัว จริงๆแล้วในรหัสฉันไม่ได้ใช้เมทริกซ์เลย แก้ไขสมการทั้งหมดและค่าทั้งหมดเป็นค่าพื้นฐาน (สองเท่า)

ซอร์สโค้ดทำงานและมีกิจกรรมสาธิต ขออภัยที่ไม่มี javadoc ในบางสถานที่ฉันจะตามทัน

คุณไม่ควรคำนวณความเร็วจากการเปลี่ยนตำแหน่งต่อเวลา GPS อาจมีตำแหน่งที่ไม่ถูกต้อง แต่มีความเร็วที่แม่นยำ (สูงกว่า 5km / h) ดังนั้นใช้ความเร็วจากตำแหน่ง GPS และยิ่งไปกว่านั้นคุณไม่ควรทำเช่นนั้นด้วยแน่นอนแม้ว่ามันจะได้ผลเกือบทุกครั้ง

ตำแหน่ง GPS ตามที่ส่งมอบมีการกรองคาลมานแล้วคุณอาจไม่สามารถปรับปรุงได้ในการประมวลผลภายหลังโดยปกติแล้วคุณจะไม่ได้ข้อมูลที่เหมือนกันเช่นชิป GPS

คุณสามารถทำให้ราบรื่น แต่นี่ยังแนะนำข้อผิดพลาด

เพียงตรวจสอบให้แน่ใจว่าคุณลบตำแหน่งเมื่ออุปกรณ์หยุดนิ่งนี่จะเป็นการลบตำแหน่งการกระโดดซึ่งอุปกรณ์ / การกำหนดค่าบางอย่างไม่ได้ลบ

ฉันมักจะใช้เครื่องวัดความเร็ว การเปลี่ยนแปลงตำแหน่งอย่างฉับพลันในช่วงเวลาสั้น ๆ หมายถึงการเร่งความเร็วสูง ถ้านี่ไม่ได้สะท้อนในมาตรวัดความเร่ง (accelerometer telemetry) มันเกือบจะแน่นอนเนื่องจากมีการเปลี่ยนแปลงในดาวเทียม "สามดวงที่ดีที่สุด" ที่ใช้ในการคำนวณตำแหน่ง

เมื่อสินทรัพย์หยุดพักและกระโดดเนื่องจาก GPS teleporting หากคุณคำนวณ centroid อย่างต่อเนื่องคุณจะทำการตัดชุดกระสุนที่ใหญ่และใหญ่ขึ้นอย่างมีประสิทธิภาพ

ในการทำเช่นนี้เมื่อสินทรัพย์ไม่ได้หยุดนิ่งคุณจะต้องประเมินตำแหน่งและทิศทางถัดไปที่น่าจะเป็นไปตามความเร็วส่วนหัวและเส้นตรงและการหมุน (ถ้าคุณมีไจโร) ข้อมูลการเร่งความเร็ว นี่คือสิ่งที่ตัวกรอง K โด่งดังไม่มากก็น้อย คุณสามารถซื้อฮาร์ดแวร์ทั้งหมดได้ในราคา $ 150 ใน AHRS ที่มีทุกอย่างยกเว้นโมดูล 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 ใหญ่ที่สุดคุณจะแทนที่ point B ด้วย D ถ้า BD = min {AB, BC, BD, BE}

การปรับให้เรียบนี้จะกำหนดค่าผิดปกติและสามารถเพิ่มได้โดยใช้จุดกึ่งกลางของ BD แทนจุด D เพื่อทำให้เส้นตำแหน่งเรียบขึ้น ระยะทางของคุณอาจแตกต่างกันและมีวิธีแก้ไขปัญหาทางคณิตศาสตร์ที่เข้มงวดมากขึ้น

สำหรับสี่เหลี่ยมจัตุรัสอย่างน้อยนี่เป็นอีกสองสิ่งที่ต้องทดสอบ:

1. เพียงเพราะขนาดกำลังพอดีน้อยที่สุดไม่ได้หมายความว่ามันต้องเป็นแบบเชิงเส้น คุณสามารถใส่เส้นโค้งกำลังสองกับข้อมูลได้อย่างน้อยที่สุดแล้วนี่จะเหมาะกับสถานการณ์ที่ผู้ใช้กำลังเร่ง (โปรดทราบว่าอย่างน้อยกำลังสองพอดีฉันหมายถึงการใช้พิกัดเป็นตัวแปรตามและเวลาเป็นตัวแปรอิสระ)

2. คุณสามารถลองถ่วงน้ำหนักจุดข้อมูลตามรายงานความแม่นยำ เมื่อความแม่นยำมีน้ำหนักเบาข้อมูลเหล่านั้นจะลดลง

3. อีกสิ่งหนึ่งที่คุณอาจต้องการลองก็คือแทนที่จะแสดงจุดเดียวถ้าความแม่นยำต่ำแสดงวงกลมหรือบางสิ่งที่บ่งบอกถึงช่วงที่ผู้ใช้อาจยึดตามความถูกต้องที่รายงาน (นี่คือสิ่งที่แอปพลิเคชั่น Google Maps ในตัวของ iPhone ทำ)

คุณยังสามารถใช้อิสระได้ ฟีดในค่าที่คุณมีและสอดแทรกจุดระหว่างจุดที่คุณรู้จัก การเชื่อมโยงสิ่งนี้เข้ากับรูปสี่เหลี่ยมจัตุรัสที่น้อยที่สุดค่าเฉลี่ยเคลื่อนที่หรือตัวกรองคาลมาน (ดังที่ได้กล่าวไว้ในคำตอบอื่น ๆ ) ช่วยให้คุณสามารถคำนวณคะแนนระหว่างจุดที่คุณรู้จัก

ความสามารถในการสอดแทรกค่าระหว่างค่าที่รู้จักกันของคุณจะช่วยให้คุณมีการเปลี่ยนแปลงที่ราบรื่นและ / / เหตุผล / การประมาณของข้อมูลที่จะนำเสนอหากคุณมีความเที่ยงตรงสูงกว่า http://en.wikipedia.org/wiki/Spline_interpolation

เส้นโค้งที่แตกต่างกันมีลักษณะแตกต่างกัน ของที่ฉันเคยเห็นใช้บ่อยที่สุดคือ Akima และ Cubic splines

อัลกอริธึมที่ต้องพิจารณาอีกประการหนึ่งคืออัลกอริธึมการทำให้เข้าใจง่ายของสาย Ramer-Douglas-Peucker ( http://en.wikipedia.org/wiki/Ramer-Douglas-Peucker_algorithm )

กลับไปที่ตัวกรองคาลมาน ... ฉันพบการติดตั้งตัวกรองคาลมานสำหรับข้อมูล GPS ที่นี่: http://github.com/lacker/ikalmanฉันยังไม่ได้ลองใช้ แต่ดูเหมือนว่าจะมีแนวโน้ม

แมปกับ CoffeeScript หากใครสนใจ ** แก้ไข -> ขอโทษด้วยการใช้กระดูกสันหลังด้วยเช่นกัน แต่คุณก็เข้าใจ

แก้ไขเล็กน้อยเพื่อรับสัญญาณด้วย attribs

{ละติจูด: 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 จาก @Stochastically เป็น 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
    /// /programming/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
        }
    }
}

นี่คือการใช้งานจาวาสคริปต์ของการใช้งานจาวาของ @ Stochastically สำหรับทุกคนที่ต้องการ:

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