mirror of
https://gitlab.com/eql/lqml.git
synced 2025-12-06 10:31:34 -08:00
50 lines
1.5 KiB
Common Lisp
50 lines
1.5 KiB
Common Lisp
;;; A 'Kalman' filter is a much more sophisticated filter than a simple
|
|
;;; 'low-pass' filter.
|
|
;;;
|
|
;;; It takes a theoretically well founded approach, which means: it makes an
|
|
;;; 'educated guess' of the next position to be expected, and 'corrects' it
|
|
;;; with actual GPS data, considering the accuracy and the speed of current raw
|
|
;;; GPS data.
|
|
;;;
|
|
;;; For most use cases this is already sufficiently precise.
|
|
;;;
|
|
;;; If you need more precision, you would need to integrate e.g. accelerometer
|
|
;;; data into the Kalman filter.
|
|
|
|
(defpackage :kalman
|
|
(:use :cl)
|
|
(:nicknames :kal)
|
|
(:export
|
|
#:*lat*
|
|
#:*lon*
|
|
#:*timestamp*
|
|
#:*variance*
|
|
#:filter
|
|
#:reset))
|
|
|
|
(in-package :kal)
|
|
|
|
(defvar *lat* nil)
|
|
(defvar *lon* nil)
|
|
(defvar *timestamp* nil)
|
|
(defvar *variance* nil)
|
|
|
|
(defun reset ()
|
|
(setf *variance* nil))
|
|
|
|
(defun filter (lat lon accuracy speed)
|
|
(when (and lat lon accuracy speed)
|
|
(setf accuracy (max 1 accuracy))
|
|
(let ((timestamp (get-internal-real-time)))
|
|
(if *variance*
|
|
(let ((time-inc (- timestamp *timestamp*)))
|
|
(incf *variance* (/ (* time-inc (expt speed 2)) 1000))
|
|
(setf *timestamp* timestamp)
|
|
(let ((k (/ *variance* (+ *variance* (expt accuracy 2)))))
|
|
(incf *lat* (* k (- lat *lat*)))
|
|
(incf *lon* (* k (- lon *lon*)))
|
|
(setf *variance* (* (- 1 k) *variance*))))
|
|
(setf *lat* lat
|
|
*lon* lon
|
|
*timestamp* timestamp
|
|
*variance* (expt accuracy 2))))))
|