Skip to content

Commit 8ff70ad

Browse files
pavpanchekhapavpanchekha
authored
Merge pull request #1407 from herbie-fp/kalman-filter-benchmarks
Kalman filter benchmarks
2 parents 5cd0f16 + 72f30a2 commit 8ff70ad

File tree

1 file changed

+235
-0
lines changed

1 file changed

+235
-0
lines changed

‎bench/physics/kalman.fpcore‎

Lines changed: 235 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,235 @@
1+
(FPCore (dt r)
2+
:name "Kalman filter per K"
3+
:pre (and (> dt 0) (> r 0))
4+
; initializing matrices
5+
(let ([p00 25.0]
6+
[p01 0.0]
7+
[p02 0.0]
8+
[p10 0.0]
9+
[p11 10.0]
10+
[p12 0.0]
11+
[p20 0.0]
12+
[p21 0.0]
13+
[p22 1.0]
14+
15+
[f00 1.0]
16+
[f01 dt]
17+
[f02 (* 0.5 (* dt dt))]
18+
[f10 0.0]
19+
[f11 1.0]
20+
[f12 dt]
21+
[f20 0.0]
22+
[f21 0.0]
23+
[f22 1.0]
24+
25+
[q00 (* 0.25 (* dt (* dt (* dt dt))))]
26+
[q01 (* 0.5 (* dt (* dt dt)))]
27+
[q02 (* 0.5 (* dt dt))]
28+
[q10 (* 0.5 (* dt (* dt dt)))]
29+
[q11 (* dt dt)]
30+
[q12 dt]
31+
[q20 (* 0.5 (* dt dt))]
32+
[q21 dt]
33+
[q22 1])
34+
; axbT_33(P, F, P)
35+
(let ([p00* (+ (+ (* p00 f00) (* p01 f01)) (* p02 f02))]
36+
[p01* (+ (+ (* p00 f10) (* p01 f11)) (* p02 f12))]
37+
[p02* (+ (+ (* p00 f20) (* p01 f21)) (* p02 f22))]
38+
[p10* (+ (+ (* p10 f00) (* p11 f01)) (* p12 f02))]
39+
[p11* (+ (+ (* p10 f10) (* p11 f11)) (* p12 f12))]
40+
[p12* (+ (+ (* p10 f20) (* p11 f21)) (* p12 f22))]
41+
[p20* (+ (+ (* p20 f00) (* p21 f01)) (* p22 f02))]
42+
[p21* (+ (+ (* p20 f10) (* p21 f11)) (* p22 f12))]
43+
[p22* (+ (+ (* p20 f20) (* p21 f21)) (* p22 f22))])
44+
; axb_33(F, P, P)
45+
(let ([p00** (+ (+ (* f00 p00*) (* f01 p10*)) (* f02 p20*))]
46+
[p01** (+ (+ (* f00 p01*) (* f01 p11*)) (* f02 p21*))]
47+
[p02** (+ (+ (* f00 p02*) (* f01 p12*)) (* f02 p22*))]
48+
[p10** (+ (+ (* f10 p00*) (* f11 p10*)) (* f12 p20*))]
49+
[p11** (+ (+ (* f10 p01*) (* f11 p11*)) (* f12 p21*))]
50+
[p12** (+ (+ (* f10 p02*) (* f11 p12*)) (* f12 p22*))]
51+
[p20** (+ (+ (* f20 p00*) (* f21 p10*)) (* f22 p20*))]
52+
[p21** (+ (+ (* f20 p01*) (* f21 p11*)) (* f22 p21*))]
53+
[p22** (+ (+ (* f20 p02*) (* f21 p12*)) (* f22 p22*))])
54+
; a_add_b_33(P, Q, P)
55+
(let ([p00*** (+ p00** q00)]
56+
[p01*** (+ p01** q01)]
57+
[p02*** (+ p02** q02)]
58+
[p10*** (+ p10** q10)]
59+
[p11*** (+ p11** q11)]
60+
[p12*** (+ p12** q12)]
61+
[p20*** (+ p20** q20)]
62+
[p21*** (+ p21** q21)]
63+
[p22*** (+ p22** q22)])
64+
; update_K
65+
(let ([K0 (/ p00*** (+ p00*** r))]
66+
[K1 (/ p10*** (+ p00*** r))]
67+
[K2 (/ p20*** (+ p00*** r))])
68+
K0))))))
69+
70+
(FPCore (x0 x1 x2 dt r sensor)
71+
:name "Kalman filter per x"
72+
:pre (and (> dt 0) (> r 0) (> sensor 0))
73+
; initializing matrices
74+
(let ([p00 25.0]
75+
[p01 0.0]
76+
[p02 0.0]
77+
[p10 0.0]
78+
[p11 10.0]
79+
[p12 0.0]
80+
[p20 0.0]
81+
[p21 0.0]
82+
[p22 1.0]
83+
84+
[f00 1.0]
85+
[f01 dt]
86+
[f02 (* 0.5 (* dt dt))]
87+
[f10 0.0]
88+
[f11 1.0]
89+
[f12 dt]
90+
[f20 0.0]
91+
[f21 0.0]
92+
[f22 1.0]
93+
94+
[q00 (* 0.25 (* dt (* dt (* dt dt))))]
95+
[q01 (* 0.5 (* dt (* dt dt)))]
96+
[q02 (* 0.5 (* dt dt))]
97+
[q10 (* 0.5 (* dt (* dt dt)))]
98+
[q11 (* dt dt)]
99+
[q12 dt]
100+
[q20 (* 0.5 (* dt dt))]
101+
[q21 dt]
102+
[q22 1])
103+
; axbT_33(P, F, P)
104+
(let ([p00* (+ (+ (* p00 f00) (* p01 f01)) (* p02 f02))]
105+
[p01* (+ (+ (* p00 f10) (* p01 f11)) (* p02 f12))]
106+
[p02* (+ (+ (* p00 f20) (* p01 f21)) (* p02 f22))]
107+
[p10* (+ (+ (* p10 f00) (* p11 f01)) (* p12 f02))]
108+
[p11* (+ (+ (* p10 f10) (* p11 f11)) (* p12 f12))]
109+
[p12* (+ (+ (* p10 f20) (* p11 f21)) (* p12 f22))]
110+
[p20* (+ (+ (* p20 f00) (* p21 f01)) (* p22 f02))]
111+
[p21* (+ (+ (* p20 f10) (* p21 f11)) (* p22 f12))]
112+
[p22* (+ (+ (* p20 f20) (* p21 f21)) (* p22 f22))])
113+
; axb_33(F, P, P)
114+
(let ([p00** (+ (+ (* f00 p00*) (* f01 p10*)) (* f02 p20*))]
115+
[p01** (+ (+ (* f00 p01*) (* f01 p11*)) (* f02 p21*))]
116+
[p02** (+ (+ (* f00 p02*) (* f01 p12*)) (* f02 p22*))]
117+
[p10** (+ (+ (* f10 p00*) (* f11 p10*)) (* f12 p20*))]
118+
[p11** (+ (+ (* f10 p01*) (* f11 p11*)) (* f12 p21*))]
119+
[p12** (+ (+ (* f10 p02*) (* f11 p12*)) (* f12 p22*))]
120+
[p20** (+ (+ (* f20 p00*) (* f21 p10*)) (* f22 p20*))]
121+
[p21** (+ (+ (* f20 p01*) (* f21 p11*)) (* f22 p21*))]
122+
[p22** (+ (+ (* f20 p02*) (* f21 p12*)) (* f22 p22*))])
123+
; a_add_b_33(P, Q, P)
124+
(let ([p00*** (+ p00** q00)]
125+
[p01*** (+ p01** q01)]
126+
[p02*** (+ p02** q02)]
127+
[p10*** (+ p10** q10)]
128+
[p11*** (+ p11** q11)]
129+
[p12*** (+ p12** q12)]
130+
[p20*** (+ p20** q20)]
131+
[p21*** (+ p21** q21)]
132+
[p22*** (+ p22** q22)])
133+
; update_K
134+
(let ([K0 (/ p00*** (+ p00*** r))]
135+
[K1 (/ p10*** (+ p00*** r))]
136+
[K2 (/ p20*** (+ p00*** r))])
137+
; predict_x
138+
(let ([x0* (+ x0 (+ x1 (* 0.5 (* dt (* dt x2)))))]
139+
[x1* (+ x1 x2)]
140+
[x2* x2])
141+
(let ([y (- sensor x0*)])
142+
; update_x
143+
(let ([x0** (+ x0* (* K0 y))]
144+
[x1** (+ x1* (* K1 y))]
145+
[x2** (+ x2* (* K2 y))])
146+
x0**)))))))))
147+
148+
(FPCore (dt r)
149+
:name "Kalman filter per P"
150+
:pre (and (> dt 0) (> r 0))
151+
; initializing matrices
152+
(let ([p00 25.0]
153+
[p01 0.0]
154+
[p02 0.0]
155+
[p10 0.0]
156+
[p11 10.0]
157+
[p12 0.0]
158+
[p20 0.0]
159+
[p21 0.0]
160+
[p22 1.0]
161+
162+
[f00 1.0]
163+
[f01 dt]
164+
[f02 (* 0.5 (* dt dt))]
165+
[f10 0.0]
166+
[f11 1.0]
167+
[f12 dt]
168+
[f20 0.0]
169+
[f21 0.0]
170+
[f22 1.0]
171+
172+
[q00 (* 0.25 (* dt (* dt (* dt dt))))]
173+
[q01 (* 0.5 (* dt (* dt dt)))]
174+
[q02 (* 0.5 (* dt dt))]
175+
[q10 (* 0.5 (* dt (* dt dt)))]
176+
[q11 (* dt dt)]
177+
[q12 dt]
178+
[q20 (* 0.5 (* dt dt))]
179+
[q21 dt]
180+
[q22 1])
181+
; axbT_33(P, F, P)
182+
(let ([p00* (+ (+ (* p00 f00) (* p01 f01)) (* p02 f02))]
183+
[p01* (+ (+ (* p00 f10) (* p01 f11)) (* p02 f12))]
184+
[p02* (+ (+ (* p00 f20) (* p01 f21)) (* p02 f22))]
185+
[p10* (+ (+ (* p10 f00) (* p11 f01)) (* p12 f02))]
186+
[p11* (+ (+ (* p10 f10) (* p11 f11)) (* p12 f12))]
187+
[p12* (+ (+ (* p10 f20) (* p11 f21)) (* p12 f22))]
188+
[p20* (+ (+ (* p20 f00) (* p21 f01)) (* p22 f02))]
189+
[p21* (+ (+ (* p20 f10) (* p21 f11)) (* p22 f12))]
190+
[p22* (+ (+ (* p20 f20) (* p21 f21)) (* p22 f22))])
191+
; axb_33(F, P, P)
192+
(let ([p00** (+ (+ (* f00 p00*) (* f01 p10*)) (* f02 p20*))]
193+
[p01** (+ (+ (* f00 p01*) (* f01 p11*)) (* f02 p21*))]
194+
[p02** (+ (+ (* f00 p02*) (* f01 p12*)) (* f02 p22*))]
195+
[p10** (+ (+ (* f10 p00*) (* f11 p10*)) (* f12 p20*))]
196+
[p11** (+ (+ (* f10 p01*) (* f11 p11*)) (* f12 p21*))]
197+
[p12** (+ (+ (* f10 p02*) (* f11 p12*)) (* f12 p22*))]
198+
[p20** (+ (+ (* f20 p00*) (* f21 p10*)) (* f22 p20*))]
199+
[p21** (+ (+ (* f20 p01*) (* f21 p11*)) (* f22 p21*))]
200+
[p22** (+ (+ (* f20 p02*) (* f21 p12*)) (* f22 p22*))])
201+
; a_add_b_33(P, Q, P)
202+
(let ([p00*** (+ p00** q00)]
203+
[p01*** (+ p01** q01)]
204+
[p02*** (+ p02** q02)]
205+
[p10*** (+ p10** q10)]
206+
[p11*** (+ p11** q11)]
207+
[p12*** (+ p12** q12)]
208+
[p20*** (+ p20** q20)]
209+
[p21*** (+ p21** q21)]
210+
[p22*** (+ p22** q22)])
211+
; update_K
212+
(let ([K0 (/ p00*** (+ p00*** r))]
213+
[K1 (/ p10*** (+ p00*** r))]
214+
[K2 (/ p20*** (+ p00*** r))])
215+
; update_P
216+
(let ([eyekh00 (- 1 K0)]
217+
[eyekh01 0.0]
218+
[eyekh02 0.0]
219+
[eyekh10 (- K1)]
220+
[eyekh11 1.0]
221+
[eyekh12 0.0]
222+
[eyekh20 (- K2)]
223+
[eyekh21 0.0]
224+
[eyekh22 1.0])
225+
; axb_33(&eyekh, P, P)
226+
(let ([p00**** (+ (+ (* eyekh00 p00***) (* eyekh01 p10***)) (* eyekh02 p20***))]
227+
[p01**** (+ (+ (* eyekh00 p01***) (* eyekh01 p11***)) (* eyekh02 p21***))]
228+
[p02**** (+ (+ (* eyekh00 p02***) (* eyekh01 p12***)) (* eyekh02 p22***))]
229+
[p10**** (+ (+ (* eyekh10 p00***) (* eyekh11 p10***)) (* eyekh12 p20***))]
230+
[p11**** (+ (+ (* eyekh10 p01***) (* eyekh11 p11***)) (* eyekh12 p21***))]
231+
[p12**** (+ (+ (* eyekh10 p02***) (* eyekh11 p12***)) (* eyekh12 p22***))]
232+
[p20**** (+ (+ (* eyekh20 p00***) (* eyekh21 p10***)) (* eyekh22 p20***))]
233+
[p21**** (+ (+ (* eyekh20 p01***) (* eyekh21 p11***)) (* eyekh22 p21***))]
234+
[p22**** (+ (+ (* eyekh20 p02***) (* eyekh21 p12***)) (* eyekh22 p22***))])
235+
p00****))))))))

0 commit comments

Comments
 (0)