kalman

defmodule Kalman do
  @moduledoc """
  Implements the Kalman Filter as `filter/1`.

  `filter/1` is meant to be used recursively 
  """
  # A = 1  # No process innovation
  # B = 0  # No control input
  # C = 1  # Measurement Dynamics
  # Q = 0.005  # Process covariance
  # R = 1  # Measurement covariance
  # x = -50  # Initial estimate
  # P = 1  # Initial covariance

  @doc """
  `filter/1` takes a tuple `{measurement, current_state_estimate, current_prop_estimate}`, applies the prediction step, observation step and update step of the Kalman Filter.

  It then returns the updated values in a map on the form `%{current_state_estimate: current_state_estimate, current_prop_estimate: current_prop_estimate}`.

  The returned map can then be used with a new measured value, recursively.

  ## Examples

      iex> l = [-77, -76, -63, -74, -66, -75, -77, -63, -77, -63]
      iex> Enum.scan(l, %{current_prop_estimate: 1, current_state_estimate: -55},
      ...>   fn measurement, %{current_prop_estimate: cpe, current_state_estimate: cse} ->
      ...>     Kalman.filter({measurement, cse, cpe})
      ...>   end)
      [
        %{current_prop_estimate: 0.5012468827930174, current_state_estimate: -66.02743142144638},
        %{current_prop_estimate: 0.33609821110752397, current_state_estimate: -69.37919388084535},
        %{current_prop_estimate: 0.25434245477505607, current_state_estimate: -67.75669404970513},
        %{current_prop_estimate: 0.20593481446742765, current_state_estimate: -69.04240810224249},
        %{current_prop_estimate: 0.17419171696719063, current_state_estimate: -68.51244581119798},
        %{current_prop_estimate: 0.15196147868818213, current_state_estimate: -69.49830413879805},
        %{current_prop_estimate: 0.1356669876910271, current_state_estimate: -70.51603661886156},
        %{current_prop_estimate: 0.12331994281325659, current_state_estimate: -69.58915941284121},
        %{current_prop_estimate: 0.1137265574632268, current_state_estimate: -70.43196880072755},
        %{current_prop_estimate: 0.10612652097260095, current_state_estimate: -69.64323980792942}
      ]
  """
  @spec filter(input_tuple :: {number, number, number}) :: map
  def filter({measurement, current_state_estimate, current_prop_estimate}) do
    do_estimate({measurement, current_state_estimate, current_prop_estimate})
  end

  @spec do_estimate({number, number, number}, number, number, number, number, number, number) ::
          map
  defp do_estimate(
         {measurement, x, p},
         # process dynamics
         a \\ 1,
         # control dynamics
         b \\ 0,
         # measurement dynamics
         c \\ 1,
         # process covariance
         q \\ 0.005,
         # measurement covariance,
         r \\ 1,
         control_input \\ 0
       ) do
    # Prediction step
    predicted_state_estimate = a * x + b * control_input
    predicted_prob_estimate = a * p * a + q

    # Observation step
    innovation = measurement - c * predicted_state_estimate
    innovation_covariance = c * predicted_prob_estimate * c + r

    # Update step
    kalman_gain = predicted_prob_estimate * c * 1 / innovation_covariance
    current_state_estimate = predicted_state_estimate + kalman_gain * innovation

    current_prop_estimate = (1 - kalman_gain * c) * predicted_prob_estimate

    %{
      current_state_estimate: current_state_estimate,
      current_prop_estimate: current_prop_estimate
    }
  end
end