my.signal_theory.kalman source code

1 /**
2 Copyright: Copyright (c) 2020, Joakim Brännström. All rights reserved.
3 License: $(LINK2 http://www.boost.org/LICENSE_1_0.txt, Boost Software License 1.0)
4 Author: Joakim Brännström (joakim.brannstrom@gmx.com)
5 */
6 module my.signal_theory.kalman;
7 
8 import core.time : Duration, dur;
9 import std.math : abs;
10 import std.range : isOutputRange;
11 
12 @safe:
13 
14 /// Kalman filter for a unidimensional models.
15 struct KalmanFilter {
16     double currentEstimate = 0;
17     double estimateError = 0;
18     double kalmanGain = 0;
19     double lastEstimate = 0;
20     double measurementError = 0;
21     double q = 0;
22 
23     /**
24      * Params:
25      *  measurementError = measurement uncertainty. How much the measurements
26      *      is expected to vary.
27      *  estimationError = estimation uncertainty. Adjusted over time by the
28      *      Kalman Filter but can be initialized to mea_e.
29      *  q = process variance. usually a small number [0.001, 1]. How fast the
30      *      measurement moves. Recommended is 0.001, tune as needed.
31      */
32     this(double measurementError, double estimateError, double q) {
33         this.measurementError = measurementError;
34         this.estimateError = estimateError;
35         this.q = q;
36     }
37 
38     void updateEstimate(double mea) {
39         kalmanGain = estimateError / (estimateError + measurementError);
40         currentEstimate = lastEstimate + kalmanGain * (mea - lastEstimate);
41         estimateError = (1.0 - kalmanGain) * estimateError + abs(lastEstimate - currentEstimate) * q;
42         lastEstimate = currentEstimate;
43     }
44 
45     string toString() @safe const {
46         import std.array : appender;
47 
48         auto buf = appender!string;
49         toString(buf);
50         return buf.data;
51     }
52 
53     void toString(Writer)(ref Writer w) const if (isOutputRange!(Writer, char)) {
54         import std.format : formattedWrite;
55 
56         formattedWrite(w,
57                 "KalmanFilter(measurementError:%s estimateError:%s q:%s currentEstimate:%s lastEstimate:%s gain:%s",
58                 measurementError, estimateError, q, currentEstimate, lastEstimate, kalmanGain);
59     }
60 }
61 
62 @("shall instantiate and run a kalman filter")
63 unittest {
64     import std.stdio : writefln, writeln;
65     import my.signal_theory.simulate;
66 
67     Simulator sim;
68 
69     const period = sim.period;
70     const double ticks = cast(double) 1.dur!"seconds"
71         .total!"nsecs" / cast(double) period.total!"nsecs";
72     const clamp = period.total!"nsecs" / 2;
73 
74     auto kf = KalmanFilter(2, 2, 0.01);
75 
76     while (sim.currTime < 1000.dur!"msecs") {
77         sim.tick!"nsecs"(a => kf.updateEstimate(a.total!"nsecs"), () => -kf.currentEstimate);
78         if (sim.updated) {
79             const diff = sim.targetTime - sim.wakeupTime;
80             //writefln!"time[%s] pv[%s] diff[%s]"(sim.currTime, sim.pv, diff);
81             //writeln(kf);
82         }
83     }
84 
85     assert(abs(sim.pv.total!"msecs") < 100);
86     assert(abs(kf.currentEstimate) < 18000.0);
87 }