VAR
  long process_variance, drift_variance, corrector_variance
  long value, bias
  long P_00, P_01, P_10, P_11


OBJ
  m: "DynamicMathLib"


PUB init(process_var, drift_var, corrector_var)
  process_variance := process_var
  drift_variance := drift_var
  corrector_variance := corrector_var

  value := 0
  bias := 0

  P_00 := process_var
  P_01 := 0.0
  P_10 := 0.0
  P_11 := drift_var


PUB get_value | E, K_0
  return value


PUB get_bias
  return bias


PUB update(corrector, sensor) | E, K_0, K_1, T_00, T_01, T_10, T_11, err
  ' State estimate:
  '   x = [ value ]
  '       [ bias  ]
  '
  ' Forcing value:
  '   u = [ sensor ]
  '
  ' Measurement:
  '   z = [ corrector ]
  '
  ' Measurement:
  '
  ' Process evolution matrix:
  '   A = [ 1, -1 ]    value = value - bias
  '       [ 0,  1 ]    bias = bias
  '
  ' Forcing matrix:
  '   B = [ 1 ]        value += sensor
  '       [ 0 ]        bias += 0
  '
  ' Measurement matrix:
  '   H = [ 1, 0 ]     corrector = value
  '
  ' Estimate covariance:
  '   P = [ P_00, P_01 ]
  '       [ P_10, P_11 ]
  '
  ' Process covariance:
  '   Q = [ process_variance, 0 ]
  '       [ 0,   drift_variance ]
  '
  ' Mwasurement "co"variance:
  '   R = [ corrector_variance ]

  '
  ' E = H P H' + R
  '

  E := m.FAdd(P_00, corrector_variance)

  '
  ' K = P H' / E
  '

  K_0 := m.FDiv(P_00, E)
  K_1 := m.FDiv(P_10, E)

  '
  ' x += K (z - H x)
  '

  err := m.FSub(corrector, value)
  value := m.FAdd(value, m.FMul(K_0, err))
  bias := m.FAdd(bias, m.FMul(K_1, err))

  '
  ' P -= K H P
  '

  T_00 := m.FSub(P_00, m.FMul(K_0, P_00))
  T_01 := m.FSub(P_01, m.FMul(K_0, P_01))
  T_10 := m.FSub(P_10, m.FMul(K_1, P_00))
  T_11 := m.FSub(P_11, m.FMul(K_1, P_01))

  '
  ' x = A x + B z
  '

  value := m.FAdd(value, m.FSub(sensor, bias))

  '
  ' P = A P A' + Q
  '

  P_00 := m.FAdd(m.FAdd(m.FSub(m.FSub(T_11, T_10), T_01), T_00), process_variance)
  P_01 := m.FSub(T_01, T_11)
  P_10 := m.FSub(T_10, T_11)
  P_11 := m.FAdd(T_11, drift_variance)