Calibration!

.github/workflows/docs.yml

@@ -12,7 +12,7 @@ jobs:
       - run: echo "💡 The ${{ github.repository }} repository has been cloned to the runner."
       - run: echo "🖥️ The workflow is now ready to test your code on the runner."
       - name: Check for changes to docs. Relies on empty being falsey
-        run: git diff --name-only cpp-gen $(git merge-base cpp-gen origin/master) | grep "^docs/designs/"
+        run: git diff --name-only calibration $(git merge-base calibration origin/master) | grep "^docs/designs/"
       - run: echo "🍏 This job's status is ${{ job.status }}."
   docs-diff:
     runs-on: ubuntu-latest
@@ -27,5 +27,5 @@ jobs:
       - run: echo "💡 The ${{ github.repository }} repository has been cloned to the runner."
       - run: echo "🖥️ The workflow is now ready to test your code on the runner."
       - name: Check for changes to docs. Relies on empty being falsey
-        run: git diff --name-only cpp-gen $(git merge-base cpp-gen origin/master) | grep "^docs" | grep -v "^docs/designs/"
+        run: git diff --name-only calibration $(git merge-base calibration origin/master) | grep "^docs" | grep -v "^docs/designs/"
       - run: echo "🍏 This job's status is ${{ job.status }}."

docs/designs/calibration.md

@@ -0,0 +1,122 @@
+# Calibration
+
+Author: Buck Baskin @bebaskin
+Created: 2023-05-06
+Updated: 2023-05-06
+Parent Design: [designs/cpp_library_for_model_evaluation.md](../designs/cpp_library_for_model_evaluation.md)
+See Also: [designs/python_library_for_model_evaluation.md](../designs/python_library_for_model_evaluation.md)
+Status: Refactor/Cleanup
+
+## Overview
+
+FormaK aims to combine symbolic modeling for fast, efficient system modelling
+with code generation to create performant code that is easy to use.
+
+This design provides an extension to the fifth of the Five Keys
+"C++ interfaces to support a variety of model uses".
+
+When defining a model, in theory everything can be estimated as a state or
+provided as a control input; however, having the ability to provide
+calibrations can be helpful or even essential. For example, in the NASA rocket
+dataset the position of the IMU on the rocket is calibrated ahead of time so it
+doesn't need to be estimated online.
+
+For the case of a suite of ranging sensors (say ultra-wideband time of flight
+sensors), the calibration term allows for easily setting up a single model with
+different calibrations for the known positions of each sensor in the reference
+frame. Without the calibration, each pose would be arbitrary and require
+solving a problem beyond what is suited to a Kalman Filter. With the
+calibration, the sensor model can be defined once and then calibrated multiple
+times at runtime based on how the sensors are set up for a particular use case.
+
+The Calibration use case also provides additional functionality on top of the
+Control inputs. The two categories conceptually overlap as "known" values that
+are accepted in the state update; however, the Calibration values are also
+available within the sensor model. With just a State and Control input, the
+state needs to accept control inputs as a pass through to sensor models. This
+adds a compute penalty for computations with the state.
+
+Supporting calibration is a big step forward in functionality for FormaK that
+enables a variety of new model use cases.
+
+## Solution Approach
+
+The basic approach will be to pass in calibrated values to both the process
+model and sensor model, largely following the implementation of the Control
+type (except that it will also be provided to sensor models).
+
+The key classes in the implementation are:
+- `ui.Model`: Revised to support the calibration parameters
+- `python.Model`: Revised to support the calibration parameters
+- Generated `Calibration`: (new) Generated type to provide calibration terms at runtime
+
+## Feature Tests
+
+This design was retcon'd based on a feature test of developing a model for a
+rocket launch based on NASA data.
+
+[NASA Dataset Page](https://data.nasa.gov/Aerospace/Deorbit-Descent-and-Landing-Flight-1-DDL-F1-/vicw-ivgd)
+[YouTube Video of Launch](https://www.youtube.com/watch?v=O97dPDkUGg4)
+
+The feature tests for this design are based on defining a model of the rocket
+motion and then generating Python and C++ models for the model. The
+implementation exposed a missing aspect of the FormaK model, specifically the
+introduction of the information about the pose of the IMU in the navigation
+frame of the rocket. This is provided with the dataset, but is not easily
+integrated into the model when it could only be a state (and therefore
+estimated based on an initial condition) or control (and therefore not
+available when calculating a sensor model for the IMU).
+
+## Road Map and Process
+
+1. Write a design
+2. Write a feature test(s)
+3. Build a simple prototype
+4. Pass feature tests
+5. Refactor/cleanup
+6. Build an instructive prototype (e.g. something that looks like the project vision but doesn’t need to be the full thing)
+7. Add unit testing, etc
+8. Refactor/cleanup
+9. Write up successes, retro of what changed (so I can check for this in future designs)
+
+## Post Review
+
+### 2023-05-05
+
+This design (cut out of the broader rocket model work) accidentally took
+exactly how long I wanted, specifically landing one month after the post-review
+of the previous feature. This was a happy accident because I'd originally
+intended for the rocket model itself to land in this time and instead wandered
+into this feature to design and deliver early.
+
+#### Original List of Features Expected For The Rocket Model
+
+Note: the only thing that came out of this was the calibration, partial vector
+support and partial rotation support...
+
+- Vector support
+- Rotations
+- Units
+- Map
+- Data ingestion helpers
+
+#### Design Changes - Code
+
+- Passing calibration into every function call. In the future, a managed filter can construct the calibration once, but for now they're stateless "pure" functions so they need to get the calibration passed in.
+- Update of the interface to be "don't pay for what you don't use". This applies both to optional arguments on the Python side and C++ that is conditionally generated only if the appropriate Calibration/Control is needed
+- Rotations, translations, velocities, etc got their own named generators in the model definition code. I expect this will be expanded in the future to enable easier model generation and moved into the UI code itself (e.g. rigid transforms, etc)
+- Overall, I opted to remove some of the `ui.Model` functionality that was taking a long time for a larger model in favor of faster iteration and some testing after the fact. This was a key win because I was sitting around for 5 minutes at a time at the slowest point
+- Better error messages along the way. I had enough failures and time to think to find the failure, write an error message for it and rewrite the error message the second time around
+
+#### Design Changes - Tooling
+
+- Complete rewrite of C++ code gen templates with if-based optional inclusion. This got quite messy and is still in Jinja but maybe not for long.
+- I chose to unwind some of the changes I'd made to check models for invalid cases. It was slow to execute and false-positive prone.
+- Basic testing went a long way to finding obvious stuff and not-so obvious stuff. I bet there are edge cases, but most of the basics are covered
+- Sympy `simplify` is too slow to be useful without a more careful application
+- It's helpful to not have to write to a file all the time. Tests will just dump the model writing to stdout if there's no file specified so the C++ compile calls can be run in tests
+
+#### Some Things I Learned I Didn't Know
+
+- Rotations. I have the nominal math, but still not a completely satisfying approach
+- Benchmarking is important even for smaller models

docs/thinking-with-formak.md

@@ -0,0 +1,34 @@
+# Thinking With FormaK
+
+FormaK helps take a model from concept stage to production. This is done by
+taking the model through different stages of development.
+1. Model definition - detailed model of features
+2. Model optimization - fit against data to select parameters
+3. Model compilation - compile to Python or C++
+4. Model calibration
+5. Model runtime
+
+For an Model, there are 4 inter-related concepts at play:
+- Model Definition: How does the state evolve over time?
+- State: estimated at runtime based on incoming sensor information
+- Calibration: provided once at the start of runtime
+- Control: provided as truth during runtime as the state evolves over time
+
+An EKF adds:
+- Process Model definition (see Model Definition)
+	- Process Noise: How much variability do we expect around our control input?
+- Sensor Models: How does the state relate to incoming sensor data?
+	- Sensor Noise: How much variability do we expect for the incoming sensor data?
+
+How do these relate to each other?
+- A State can be calculated online or set to a pre-determined parameter as a Calibration
+- A Control can be provided online or set to a pre-determined parameter as a Calibration
+- A Control can not be used as part of a sensor model. If you want to use a Control as a sensor model, it should be added to the State and the process model sets the State equal to the Control
+
+Note: Usually these will be referred to as a state vector or a control vector;
+however, in FormaK the exact representation can be changed under the hood so
+the State, Control, etc are just sets of symbols in a common concept
+collection. Examples of internal representation changes include: re-ordering
+the states, representing the states in a non-vector format, augmenting the
+state vector or simplifying the state vector.
+- If you want to access part or all of the State at runtime, define a sensor model to return that state member. This will allow you to access the state regardless of if the underlying state representation changes.

docs/whats-new.md

@@ -1,5 +1,18 @@
 # What's New
 
+Release Notes
+
+## 2023-05-05 Calibration
+
+Add an optional Calibration parameter for setting up multiple sensor models or
+adding sensors at known positions.
+
+Other Improvements:
+- Performance for the `ui.Model` and code generation flows is much improved
+
+Calibration Design: [designs/calibration.md](../designs/calibration.md)
+Getting Started: [getting-started.md](getting-started.md)
+
 ## 2023-04-10 C++ Source for Models
 
 Commit [`d2bec5c`](https://github.com/buckbaskin/formak/commit/d2bec5c7ea27f8092ea6d28c61917e7926fb8e72)

featuretests/BUILD

@@ -66,6 +66,17 @@ cc_formak_model(
     ),
 )
 
+cc_formak_model(
+    name = "cpp-rocket-model",
+    namespace = "featuretest",
+    pydeps = [],
+    pymain = "rocket_model/generator.py",
+    pysrcs = glob(
+        ["rocket_model/*.py"],
+        allow_empty = False,
+    ),
+)
+
 cc_test_suite(
     name = "cpp-library-for-model-evaluation",
     srcs = glob(
@@ -77,3 +88,26 @@ cc_test_suite(
         ":cpp-ekf",
     ] + CC_FEATURE_TEST_DEPS,
 )
+
+py_test_suite(
+    name = "rocket-model-py-test",
+    srcs = glob(
+        ["rocket_model/*.py"],
+        allow_empty = False,
+    ),
+    deps = [
+        "//py:formak",
+        requirement("numpy"),
+    ] + PY_FEATURE_TEST_DEPS,
+)
+
+cc_test_suite(
+    name = "rocket-model-cpp-test",
+    srcs = glob(
+        ["rocket_model/*.cpp"],
+        allow_empty = False,
+    ),
+    deps = [
+        ":cpp-rocket-model",
+    ] + CC_FEATURE_TEST_DEPS,
+)

featuretests/cpp_library_for_model_evaluation/simple_to_ekf_test.cpp

@@ -21,8 +21,7 @@ TEST(CppModel, SimpleEKF) {
 
   EXPECT_GT(next_state.z(), 0.0);
 
-  formak::SensorReading<formak::SensorId::SIMPLE, formak::Simple>
-      zero_sensor_reading;
+  formak::Simple zero_sensor_reading;
   auto next_state_and_variance = ekf.sensor_model(
       {.state = next_state, .covariance = next_variance}, zero_sensor_reading);
 

featuretests/rocket_model/ekf_cpp_test.py

@@ -0,0 +1,49 @@
+from formak import ui, cpp
+from itertools import repeat
+from model_definition import (
+    model_definition,
+    named_rotation_rate,
+    named_acceleration,
+    named_translation,
+)
+import numpy as np
+
+
+def test_cpp_EKF():
+    definition = model_definition()
+    model = definition["model"]
+
+    calibration = {
+        # orientation would need to invert a rotation matrix
+        "IMU_ori_pitch": 0.0,
+        "IMU_ori_roll": 0.0,
+        "IMU_ori_yaw": 0.0,
+        # pos_IMU_from_CON_in_CON     [m]     [-0.08035, 0.28390, -1.42333 ]
+        "IMU_pos_x": -0.08035,
+        "IMU_pos_y": 0.28390,
+        "IMU_pos_z": -1.42333,
+    }
+    calibration_map = {ui.Symbol(k): v for k, v in calibration.items()}
+
+    (reading_orientation_rate_states, _) = named_rotation_rate("IMU_reading")
+    reading_acceleration_states = sorted(
+        named_acceleration("IMU_reading").free_symbols, key=lambda x: x.name
+    )
+
+    process_noise = {
+        k: v
+        for k, v in list(zip(reading_orientation_rate_states, repeat(0.1)))
+        + list(zip(reading_acceleration_states, repeat(1.0)))
+    }
+
+    CON_position_in_global_frame = named_translation("CON_pos")
+
+    cpp.compile_ekf(
+        state_model=model,
+        process_noise=process_noise,
+        sensor_models={
+            "altitude": {ui.Symbol("altitude"): CON_position_in_global_frame[2]}
+        },
+        sensor_noises={"altitude": np.eye(1)},
+        calibration_map=calibration_map,
+    )

featuretests/rocket_model/ekf_py_test.py

@@ -0,0 +1,58 @@
+from formak import ui, python
+from itertools import repeat
+from model_definition import (
+    model_definition,
+    named_rotation_rate,
+    named_acceleration,
+    named_translation,
+)
+import numpy as np
+
+
+def test_python_EKF():
+    definition = model_definition()
+    model = definition["model"]
+
+    calibration = {
+        # orientation would need to invert a rotation matrix
+        "IMU_ori_pitch": 0.0,
+        "IMU_ori_roll": 0.0,
+        "IMU_ori_yaw": 0.0,
+        # pos_IMU_from_CON_in_CON     [m]     [-0.08035, 0.28390, -1.42333 ]
+        "IMU_pos_x": -0.08035,
+        "IMU_pos_y": 0.28390,
+        "IMU_pos_z": -1.42333,
+    }
+    calibration_map = {ui.Symbol(k): v for k, v in calibration.items()}
+
+    (reading_orientation_rate_states, _) = named_rotation_rate("IMU_reading")
+    reading_acceleration_states = sorted(
+        named_acceleration("IMU_reading").free_symbols, key=lambda x: x.name
+    )
+
+    process_noise = {
+        k: v
+        for k, v in list(zip(reading_orientation_rate_states, repeat(0.1)))
+        + list(zip(reading_acceleration_states, repeat(1.0)))
+    }
+
+    CON_position_in_global_frame = named_translation("CON_pos")
+
+    python_implementation = python.compile_ekf(
+        state_model=model,
+        process_noise=process_noise,
+        sensor_models={
+            "altitude": {ui.Symbol("altitude"): CON_position_in_global_frame[2]}
+        },
+        sensor_noises={"altitude": np.eye(1)},
+        calibration_map=calibration_map,
+        config={"compile": True, "warm_jit": True},
+    )
+
+    state_vector = np.zeros((9, 1))
+    state_covariance = np.eye(9)
+    control_vector = np.zeros((6, 1))
+
+    state_vector_next = python_implementation.process_model(
+        dt=0.01, state=state_vector, covariance=state_covariance, control=control_vector
+    )