#!/usr/bin/awk -f

### 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++) {
        ($n !~ number) ? header[n] = "d" $n : header[n] = "col_" n
    }
}

NF != 0 {
    #print "\n"
    print "\n" NR ": \t" $0
    if (NF > max_nf)
        max_nf = NF

    ### iterate over columns
    for (n=1; n<=max_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]
            (count[n]) ? mean[n] = sum[n]/count[n] : 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]
            (count[n] > 1) ? var[n] = sum_delta2[n]/(count[n] - 1) : 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]
                (count[m,n] > 1) ? meas_cov[m,n] = delta[m]*delta[n]/(count[m,n] - 1) : 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"
        }
    }
}