up

cpp/CMakeLists.txt

@@ -1,13 +1,10 @@
 cmake_minimum_required(VERSION 3.20)
 PROJECT(spiceweasel VERSION "2023.07.16")
 
-include(FetchContent)
-
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
-
 if(PROJECT_IS_TOP_LEVEL)
     cmake_host_system_information(RESULT HOST QUERY HOSTNAME)
     cmake_host_system_information(RESULT OSN QUERY OS_NAME)
@@ -25,15 +22,6 @@ if(PROJECT_IS_TOP_LEVEL)
 
     set(BUILD_EXAMPLES ON)
     set(BUILD_GTESTS ON)
-
-    # GTest -----------------
-    FetchContent_Declare(
-        gtest
-        URL https://github.com/google/googletest/archive/refs/tags/v1.14.0.zip
-    )
-    list(APPEND libs gtest)
-    FetchContent_MakeAvailable( ${libs} )
-
 else()
     message(STATUS "-> ${PROJECT_NAME} is submodule")
     set(BUILD_EXAMPLES OFF)

cpp/examples/CMakeLists.txt

@@ -1,3 +1,4 @@
+
 LIST(APPEND apps
     simple
     jacobian

cpp/gtests/CMakeLists.txt

@@ -1,3 +1,15 @@
+# GTest -----------------
+set(DOWNLOAD_EXTRACT_TIMESTAMP true)
+
+include(FetchContent)
+
+FetchContent_Declare(
+    gtest
+    URL https://github.com/google/googletest/archive/refs/tags/v1.14.0.zip
+)
+# list(APPEND libs gtest)
+FetchContent_MakeAvailable( gtest )
+
 enable_testing()
 
 list(APPEND gtests

cpp/gtests/main-gtest.cpp

@@ -1,7 +1,7 @@
 #include <gtest/gtest.h>
 #include <spiceweasel.hpp>
-#include <vector>
+// #include <vector>
 
 using namespace std;
 
-TEST(navigation, wgs84) { EXPECT_TRUE(true); }
+TEST(spiceweasel, kf) { EXPECT_TRUE(true); }

cpp/src/kf_1d.hpp

@@ -0,0 +1,32 @@
+
+#pragma once
+
+/*
+1D Kalman Filter
+*/
+class KalmanFilter1D {
+  float P{1.0};  // Initial estimate error
+  float F{1.0};  // Assuming constant velocity
+  float x{1.0};  // state estimate
+  float Q;       // processes noise
+  float R;       // measurement noise
+
+  public:
+  KalmanFilter1D(float q, float r) {
+    Q = q;
+    R = r;
+  }
+
+  float filter(float z) {
+    // Prediction
+    float xx = x * F;
+    float P = P + Q;
+
+    // Update
+    float K = P / (P + R);
+    x = xx + K * (z - xx);
+    P = (1.0f - K) * P;
+
+    return x;
+  }
+};

cpp/src/median_filter.hpp

@@ -0,0 +1,43 @@
+
+#pragma once
+
+// #include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+// #include <string.h>
+
+// namespace spiceweasel {
+
+template<typename T, int WINDOW = 3>
+class MedianFilter {
+public:
+  static_assert(WINDOW >= 3, "MedianFilter::window size must be >= 3");
+  static_assert(WINDOW % 2, "MedianFilter::window size must be odd");
+
+  MedianFilter() {
+    memset(buffer,0,WINDOW*sizeof(T));
+    memset(sorted,0,WINDOW*sizeof(T));
+  }
+
+  T filter(const T &sample) {
+    p = (p + 1) % WINDOW;
+    buffer[p] = sample;
+
+    memcpy(sorted, buffer, sizeof(buffer));
+    qsort(&sorted, WINDOW, sizeof(T), cmp);
+
+    return sorted[WINDOW / 2];
+  }
+
+protected:
+  static int cmp(const void *a, const void *b) {
+    // return (*(T *)a >= *(T *)b) ? 1 : -1;
+    return (*static_cast<T*>(a) >= *static_cast<T*>(b)) ? 1 : -1;
+  }
+
+  T buffer[WINDOW]{0.0};
+  T sorted[WINDOW]{0.0};
+  uint8_t p{0};
+};
+
+// } // namespace spiceweasel

cpp/src/spiceweasel.hpp

@@ -1,15 +1,40 @@
-
+// MIT License
+//
+// Copyright (c) 2022 Mom's Friendly Robot Company
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
 #pragma once
 
-#include <Eigen/Dense>
-#include <functional>
-using cemd = const Eigen::MatrixXd;
-using cevd = const Eigen::VectorXd;
-using emd  = Eigen::MatrixXd;
-using evd  = Eigen::VectorXd;
+#if defined(__linux__) || defined(__APPLE__)
+  #include <Eigen/Dense>
+  #include <functional>
+  using cemd = const Eigen::MatrixXd;
+  using cevd = const Eigen::VectorXd;
+  using emd  = Eigen::MatrixXd;
+  using evd  = Eigen::VectorXd;
+
+  #include "ekf.hpp"
+  #include "kf.hpp"
+#endif
 
-#include "ekf.hpp"
-#include "kf.hpp"
+#include "kf_1d.hpp"
+#include "median_filter.hpp"
 
 // #include <cstring>
 // #include <cstddef>