@@ 1,12 1,61 @@ 
-%% @doc An Extended Kalman Filter.
+%% @doc A basic Kalman Filter.
 %%
 %% Has no messaging or framework around it, just implements the core algorithm.
 %% Let's just make that work and then go from there.
 %%
+%% Maybe eventually I'll expand it to include other variations of the
+%% algorithm, but for now it's just the basic 1D model
+%%
 %% Math Stuff:
 %% https://github.com/tanguyl/numerl
 %% https://www.info.ucl.ac.be/~pvr/Brunet_26481700_Couplet_20371700_2022.pdf
 %%
+%% Algorithm source:
+%% https://www.kalmanfilter.net/
+%%
 %%
 
--module(alg_ekf).
+
+-module(alg_kalman).
+-export([run/4]).
+
+%% Kalman Gain is also written Kn
+kalman_gain(EstimateUncertainty, MeasurementUncertainty) ->
+    EstimateUncertainty / (EstimateUncertainty +
+                           MeasurementUncertainty).
+
+%% Estimate uncertainty update
+covariance_update(KalmanGain, PreviousUncertainty) ->
+    (1 - KalmanGain) * PreviousUncertainty.
+
+%% Extrapolate uncertainty from estimated uncertainty
+covariance_extrapolation(Value, ValueDeriv, Dt) ->
+    Value + ValueDeriv * (Dt * Dt).
+
+%% Update state
+update(PredictedState, Gain, Measurement) ->
+    PredictedState + Gain * (Measurement - PredictedState).
+
+predict(EstimatedState, Model, Dt) ->
+    Model(EstimatedState, Dt).
+
+run(N, InitialState, InitialEstimateUncertainty, Model) ->
+
+run2(N, State, EstimateUncertainty, Model) ->
+    if
+        N == 0 -> {};
+        true ->
+            Measurement = fdsafdsafsfas,
+            MeasurementUncertainty = fdsafdsafsa, % needs to be part of the measurement...
+            Gain = kalman_gain(EstimateUncertainty, MeasurementUncertainty),
+
+            EstimatedState = update(InitialState, Gain, Measurement),
+            EstimatedUncertainty = covariance_update(Gain, EstimateUncertainty),
+
+            PredictedState = predict(EstimatedState, Model, 1),
+            PredictedUncertainty = covariance_extrapolation(EstimatedUncertainty
+            io:format("Iter ~p, Measurement: ~p Prediction: ~p~n", [N,
+                                                                    Measurement,
+                                                                    PredictedState]),
+            run(N-1, PredictedState, MeasurementUncertainty, Model)
+    end.

          

@@ 13,9 13,14 @@ 
 -record(tuning, {p=0.0, i=0.0, d=0.0}).
 -record(pid_state, {last_err, accumulated_err}).
 
+%% @doc Create a new initial state for the PID control, that tracks the
+%% error values and such.
+-spec new_state() -> #pid_state{}.
 new_state() ->
     #pid_state{last_err = 0.0, accumulated_err = 0.0}.
 
+%% @doc Create a new set of tuning parameters.  Recomended values < 1.0.
+-spec new_tuning(float(), float(), float()) -> #tuning{}.
 new_tuning(P, I, D) ->
     #tuning {p = P, i = I, d = D}.