From 065323c7d78233e2b3d6ed1735dbd75e018d7b8f Mon Sep 17 00:00:00 2001
From: Andy Little <kwikius@yahoo.com>
Date: Sat, 22 Feb 2020 11:56:18 +0000
Subject: [PATCH] =?UTF-8?q?1d=20aircraft=20=CE=B1-=CE=B2=20filter=20(=20ka?=
 =?UTF-8?q?lman=20filter=20tutorial=20=20from=20https://www.kalmanfilter.n?=
 =?UTF-8?q?et/alphabeta.html#ex2=20)=20converted=20to=20mpusz/units?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 example/CMakeLists.txt                        |  1 +
 ...lman_filter-alpha_beta_filter_example2.cpp | 92 +++++++++++++++++++
 2 files changed, 93 insertions(+)
 create mode 100644 example/kalman_filter-alpha_beta_filter_example2.cpp

diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt
index 54ded95a..acab5452 100644
--- a/example/CMakeLists.txt
+++ b/example/CMakeLists.txt
@@ -33,3 +33,4 @@ add_example(box_example)
 add_example(capacitor_time_curve)
 add_example(clcpp_response)
 add_example(conversion_factor)
+add_example(kalman_filter-alpha_beta_filter_example2)
diff --git a/example/kalman_filter-alpha_beta_filter_example2.cpp b/example/kalman_filter-alpha_beta_filter_example2.cpp
new file mode 100644
index 00000000..2a9f894d
--- /dev/null
+++ b/example/kalman_filter-alpha_beta_filter_example2.cpp
@@ -0,0 +1,92 @@
+
+
+#include <vector>
+#include <iostream>
+#include <iomanip>
+
+#include <units/physical/si/length.h>
+#include <units/physical/si/time.h>
+#include <units/physical/si/velocity.h>
+
+/*
+    kalman filter tutorial
+    1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2
+*/
+
+using namespace units;
+using namespace units::si::literals;
+
+template <Quantity Q>
+struct state_variable{
+    Q estimated_current_state;
+    Q predicted_next_state;
+};
+
+namespace {
+   constexpr auto radar_transmit_interval = 5.0q_s;
+   constexpr double kalman_range_gain = 0.2;
+   constexpr double kalman_speed_gain = 0.1;
+}
+
+struct state{
+
+   state_variable<si::length<si::metre> > range;
+   state_variable<si::velocity<si::metre_per_second> > speed;
+
+   void estimate(const state & previous_state, const si::length<si::metre>& measurement)
+   {
+       auto const innovation = measurement - previous_state.range.predicted_next_state;
+       range.estimated_current_state = previous_state.range.predicted_next_state + kalman_range_gain * innovation;
+       speed.estimated_current_state = previous_state.speed.predicted_next_state + kalman_speed_gain * innovation / radar_transmit_interval;
+   }
+
+   void predict()
+   {
+      range.predicted_next_state = range.estimated_current_state + speed.estimated_current_state * radar_transmit_interval;
+      speed.predicted_next_state = speed.estimated_current_state;
+   }
+};
+
+int main()
+{
+   std::cout << "\n\n1d aircraft α-β filter example2 from https://www.kalmanfilter.net/alphabeta.html#ex2";
+   std::cout << "\n\n";
+
+   std::vector<si::length<si::metre> > measurements {
+      0.0q_m,    // N.B measurement[0] is unknown and unused
+      30110.0q_m, 
+      30265.0q_m,
+      30740.0q_m,
+      30750.0q_m, 
+      31135.0q_m,
+      31015.0q_m, 
+      31180.0q_m, 
+      31610.0q_m,
+      31960.0q_m, 
+      31865.0q_m
+   };
+
+   const auto num_measurements = measurements.size();
+
+   std::vector<state> track{num_measurements};
+
+   //We need an initial estimate of track[0] as there is no previous state to get a prediction from
+   track[0].range.estimated_current_state = 30'000q_m;
+   track[0].speed.estimated_current_state = 40.0q_mps;
+   
+   for ( auto n = 0U; n < num_measurements;++n){
+
+      if ( n > 0){
+          track[n].estimate(track[n-1],measurements[n]);
+      }
+      track[n].predict();
+
+      std::cout << std::fixed;
+      std::cout << "measurement[" << n << "]                    = " << std::setprecision(0) << measurements[n] <<'\n';
+      std::cout << "range.estimated_current_state[" << n << "]  = " << std::setprecision(1) << track[n].range.estimated_current_state<<'\n';
+      std::cout << "speed.estimated_current_state[" << n << "]  = " << track[n].speed.estimated_current_state  <<'\n';
+      std::cout << "range.predicted_next_state[" << n << "]     = " << track[n].range.predicted_next_state << '\n';
+      std::cout << "speed.predicted_next_state[" << n << "]     = " << track[n].speed.predicted_next_state << "\n\n";
+   }
+
+}