re-init

1 files changed, 97 insertions, 0 deletions

diff --git a/kalman_init.awk b/kalman_init.awk
new file mode 100644
index 0000000..a63a864
--- /dev/null
+++ b/kalman_init.awk
@@ -0,0 +1,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"
+        }
+    }
+}