1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
|
#!/usr/bin/awk
### kalman.awk
# kalman csv experiemnt
BEGIN {
sign = "[+-]?"
decimal = "[0-9]+[.]?[0-9]*"
fraction = "[.][0-9]*"
exponent = "([Ee]" sign "[0-9]+)?"
number = "^" sign "(" decimal "|" fraction ")" exponent "$"
}
NR == 1 {
header_nf = NF
for (n=1; n<=NF; n++) {
if ($n !~ number)
header[n] = $n
}
}
NF != 0 {
#print "\n"
print "\n" NR ": \t" $0
if (NF > max_nf)
max_nf = NF
for (n=1; n <= NF; n++) {
if ($n ~ number) {
count[n] = ($n ~ number) + (last[n] ~ number) + (last2[n] ~ number)
diff[n] = $n - last[n]
diff2[n] = 0.5*($n - 2.0*last[n] + last2[n])
sum[n] = $n + last[n] + last2[n]
if (count[n])
mean[n] = sum[n]/count[n]
else
mean[n] = sum[n]
delta[n] = $n - mean[n]
delta2[n] = delta[n]*delta[n]
sum_delta[n] = delta[n] + delta_last[n] + delta_last2[n]
sum_delta2[n] = delta2[n] + delta2_last[n] + delta2_last2[n]
if (count[n] > 1)
var[n] = sum_delta2[n]/(count[n] - 1)
else
var[n] = 0
if (count[n]) {
meas_err[n] = sqrt(var[n]/count[n])
}
### error covariance
for (m=1; m<=max_nf; m++) {
count[m,n]++
sum_xy[m,n] += 1
delta_xy[m,n] = delta[m]*delta[n]
if (count[m,n] > 1)
meas_cov[m,n] = delta[m]*delta[n]/(count[m,n] - 1)
else
meas_cov[m,n] = 0
est_err_last_cov[m,n] = 1
print meas_cov[m,n], est_err_last_cov[m,n]
}
### kalman gain
if (count[n] <= 1) {
KG[n] = 1
est_err[n] = sqrt(diff[n]*diff[n]) + 2.0*sqrt(diff2[n]*diff2[n])
est_err_last[n] = sqrt(diff[n]*diff[n]) + 2.0*sqrt(diff2[n]*diff2[n])
}
else
KG[n] = est_err_last[n]/(est_err_last[n] + meas_err[n])
### update estimate and estimated error
est[n] = est_last[n] + KG[n]*($n - est_last[n])
est_err[n] = (1.0 - KG[n])*est_err_last[n]
### visual check
printf "meas: \t%g %g %g", $n, meas_err[n], count[n]
printf "\nd1: \t%g", diff[n]
printf "\nd2: \t%g", diff2[n]
printf "\nest: \t%g %g %g", est[n], est_err[n], KG[n]
last2[n] = last[n]
last[n] = $n
diff_last2[n] = diff_last[n]
diff_last[n] = diff[n]
delta_last2[n] = delta_last[n]
delta_last[n] = delta[n]
delta2_last2[n] = delta2_last[n]
delta2_last[n] = delta2[n]
est_last[n] = est[n]
est_error_last[n] = est_error[n]
printf "\n"
}
}
}
|
