Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Minified to success #4

Open
wants to merge 1 commit into
base: enzyme-minify
Choose a base branch
from
Open

Minified to success #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,8 @@ if(KALMAN_BUILD_EXAMPLES)
#target_compile_options(example_robot1 PUBLIC)
target_compile_options(example_robot1 PUBLIC -save-temps=obj)
#add_link_options(-mllvm -enzyme-print -enzyme-loose-types=1)
target_link_libraries(example_robot1 PUBLIC LLDEnzymeFlags ${BLAS_LIBRARIES} LLDEnzymeLooseTypeFlags)
target_link_libraries(example_robot1 PUBLIC LLDEnzymeFlags ${BLAS_LIBRARIES} LLDEnzymeLooseTypeFlags LLDEnzymePrintFlags) # LLDEnzymeNoStrictAliasingFlags)
# target_link_libraries(example_robot1 PUBLIC LLDEnzymeFlags ${BLAS_LIBRARIES} LLDEnzymeLooseTypeFlags LLDEnzymePrintTypeFlags LLDEnzymePrintFlags)
# add_compile_options(-mllvm -enzyme-loose-types=1)
endif()

Expand Down
141 changes: 8 additions & 133 deletions examples/big/big.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#ifndef KALMAN_EXAMPLES1_BIG_SYSTEMMODEL_HPP_
#define KALMAN_EXAMPLES1_BIG_SYSTEMMODEL_HPP_

#include <kalman/LinearizedSystemModel.hpp>
#include <kalman/LinearizedMeasurementModel.hpp>
#include <iostream>
#include <random>
Expand All @@ -11,129 +10,8 @@ namespace KalmanExamples
namespace Big
{

const size_t n = 10;
const size_t n = 5;

/**
* @brief System state vector-type for a 3DOF planar robot
*
* This is a system state for a very simple planar robot that
* is characterized by its (x,y)-Position and angular orientation.
*
* @param T Numeric scalar type
*/
template<typename T>
class State : public Kalman::Vector<T, n>
{
public:
KALMAN_VECTOR(State, T, n)
};

template<typename T>
class Control : public Kalman::Vector<T, 1>
{
public:
KALMAN_VECTOR(Control, T, 1)
};

/**
* @brief System model for a simple planar 3DOF robot
*
* This is the system model defining how our robot moves from one
* time-step to the next, i.e. how the system state evolves over time.
*
* @param T Numeric scalar type
* @param CovarianceBase Class template to determine the covariance representation
* (as covariance matrix (StandardBase) or as lower-triangular
* coveriace square root (SquareRootBase))
*/
template<typename T, template<class> class CovarianceBase = Kalman::StandardBase>
class SystemModel : public Kalman::LinearizedSystemModel<State<T>, Control<T>, CovarianceBase>
{
public:
//! State type shortcut definition
typedef KalmanExamples::Big::State<T> S;

//! Control type shortcut definition
typedef KalmanExamples::Big::Control<T> C;

T noiseLevel = 0.1;

// SystemModel(const Kalman::Jacobian<State<T>, State<T>> A): noise(0, 1)
SystemModel()//: noise(0, 1)
{
// generator.seed(1);
auto P = this->getCovariance();
for (int i = 0; i < n; i++) {
P(i, i) = std::pow(noiseLevel, 2);
}

// in current interface, let F be set by user.

// for (int i = 0; i < n; i++) {
// for (int j = 0; j < n; j++) {
// this->F(i, j) = A(i, j);
// }
// }
}

/**
* @brief Definition of (non-linear) state transition function
*
* This function defines how the system state is propagated through time,
* i.e. it defines in which state \f$\hat{x}_{k+1}\f$ is system is expected to
* be in time-step \f$k+1\f$ given the current state \f$x_k\f$ in step \f$k\f$ and
* the system control input \f$u\f$.
*
* @param [in] x The system state in current time-step
* @param [in] u The control vector input
* @returns The (predicted) system state in the next time-step
*/
S f(const S& x, const C& u) const
{
S x_;

// TODO: avoid the copy? not so important, since we can at least update jacobians..
// Since we make a purely linear model, F plays a double role as A.
x_ = this->F * x;

// for (int i = 0; i < n; i++) {
// // write a manual matrix multiply
// x_[i] = 0;
// for (int j = 0; j < n; j++) {
// x_[i] += this->F(i, j) * x[j];
// }
// }

for (int i = 0; i < n; i++) {
x_[i] += noiseLevel;// * noise(generator);
}

return x_;
}

void updateJacobians( const S& x, const C& u )
{
// F = df/dx (Jacobian of state transition w.r.t. the state)

// in current interface, let F be set by user.

// TODO: reconsider unitary, which makes F' F = I in ekf.predict.
// this->F(0, 0) = std::cos(u[0]);
// this->F(0, 1) = -std::sin(u[0]);
// this->F(1, 0) = std::sin(u[0]);
// this->F(1, 1) = std::cos(u[0]);

// W = df/dw (Jacobian of state transition w.r.t. the noise)
this->W.setIdentity();
}
};

template<typename T>
class Measurement : public Kalman::Vector<T, n>
{
public:
KALMAN_VECTOR(Measurement, T, n)
};

/**
* @brief Measurement model for measuring orientation of a 3DOF robot
Expand All @@ -147,15 +25,12 @@ class Measurement : public Kalman::Vector<T, n>
* coveriace square root (SquareRootBase))
*/
template<typename T, template<class> class CovarianceBase = Kalman::StandardBase>
class MeasurementModel : public Kalman::LinearizedMeasurementModel<State<T>, Measurement<T>, CovarianceBase>
class MeasurementModel : public Kalman::LinearizedMeasurementModel<Eigen::Matrix<T, n, 1>, Eigen::Matrix<T, n, 1>, CovarianceBase>
{
public:
//! State type shortcut definition
typedef KalmanExamples::Big::State<T> S;

//! Measurement type shortcut definition
typedef KalmanExamples::Big::Measurement<T> M;

// mutable std::default_random_engine generator;
// mutable std::normal_distribution<T> noise;

Expand All @@ -164,9 +39,9 @@ class MeasurementModel : public Kalman::LinearizedMeasurementModel<State<T>, Mea
// generator.seed(1);
// Setup jacobians. As these are static, we can define them once
// and do not need to update them dynamically
this->H.setIdentity();
this->H *= 0.1;
this->V.setIdentity(); // TODO: what is V?
// this->H.setIdentity();
// this->H *= 0.1;
// this->V.setIdentity(); // TODO: what is V?
}

/**
Expand All @@ -179,9 +54,9 @@ class MeasurementModel : public Kalman::LinearizedMeasurementModel<State<T>, Mea
* @param [in] x The system state in current time-step
* @returns The (predicted) sensor measurement for the system state
*/
M h(const S& x) const
Eigen::Matrix<T,n, 1> h(const Eigen::Matrix<T, n, 1>& x) const
{
M measurement;
Eigen::Matrix<T,n, 1> measurement;

measurement = x;

Expand All @@ -196,4 +71,4 @@ class MeasurementModel : public Kalman::LinearizedMeasurementModel<State<T>, Mea
}
}

#endif
#endif
126 changes: 7 additions & 119 deletions examples/big/big_try.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,134 +1,22 @@
// this MUST be first, otherwise there might be problems on windows
// see: https://stackoverflow.com/questions/6563810/m-pi-works-with-math-h-but-not-with-cmath-in-visual-studio/6563891#6563891
#define _USE_MATH_DEFINES
#include <cmath>

#define EIGEN_USE_BLAS

#include "big.hpp"

#include <kalman/ExtendedKalmanFilter.hpp>

#include <iostream>
#include <random>
#include <chrono>

#include <Eigen/Core>
extern int enzyme_dup;
extern int enzyme_dupnoneed;
extern int enzyme_out;
extern int enzyme_const;

void __enzyme_autodiff(...);
#include <Eigen/Dense>

template<typename RT, typename... Args>
RT __enzyme_autodiff(void*, Args...);

using namespace KalmanExamples;

typedef float T;

typedef Big::State<T> State;
typedef Big::Control<T> Control;
typedef Big::SystemModel<T> SystemModel;

const size_t n = Big::n;

// typedef Big::PositionMeasurement<T> PositionMeasurement;
// typedef Big::OrientationMeasurement<T> OrientationMeasurement;
// typedef Big::PositionMeasurementModel<T> PositionModel;

typedef Big::MeasurementModel<T> MeasurementModel;
typedef Big::Measurement<T> Measurement;

double simulate(double* A) {//Kalman::Jacobian<State, State> A) {
// init state
State x;
for (int i = 0; i < n; i++) {
x[i] = 1;
}

// init control
Control u;
u[0] = 0.0;

// init measurement
MeasurementModel mm;

// init system
SystemModel sys;

for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
sys.F(i, j) = A[n * i + j];
}
}

// init ekf
Kalman::ExtendedKalmanFilter<State> ekf;
ekf.init(x);
ekf.P.setIdentity(); // explicitly set initial covariance, although identity is secretly already the default

double error_sum = 0.0;
const size_t N = 5;

for (size_t i = 1; i <= N; i++) {

// propagate hidden state
x = sys.f(x, u);

// propagate state estimate, read out mean
State x_ekf = ekf.predict(sys, u);

// measurement
Measurement m = mm.h(x);
ekf.update(mm, m);

error_sum += std::pow(x[0], 2);
// error_sum += std::pow(x_ekf[0] - x[0], 2);
// add a funky P-dependent term to test differentiation
// error_sum += std::pow(ekf.P(0,0), 2);

// std::cout << x[0] << "," << x[1] << "," << x_ekf[0]
// << "," << x_ekf[1] << std::endl;
}
return error_sum / (double)N;
double simulate() {
Eigen::Matrix<float, 5, 5> H;
H.setIdentity();
auto K = H.inverse();
return K(0,0);
}

int main(int argc, char **argv) {

// Kalman::Jacobian<State, State> A;
// Kalman::Jacobian<State, State> Adup;

double A[n * n];
double Adup[n * n];

for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
A[n*i + j] = 1.0;
Adup[n*i + j] = 0.0;
}
}


double delta = 0.001;
// double fx1 = simulate(1.0, A);
// double fx2 = simulate(1.0 + delta, A);
// std::cout << "fx1: " << fx1 << ", fx2: " << fx2 << std::endl;

// double df_dx1 = __enzyme_autodiff<double>((void *)simulate, enzyme_out, 1.0, enzyme_const, A);
// double df_dx2 = __enzyme_autodiff<double>((void *)simulate, enzyme_out, 1.0 + delta, enzyme_const, A);
// printf("x = %f, f(x) = %f, f'(x) = %f, f'(x) fd = %f\n", 1.0, fx1, df_dx1, (fx2 - fx1) / delta);
// printf("x = %f, f(x) = %f, f'(x) = %f", 1.0 + delta, fx2, df_dx2);

double fx1 = simulate(A);
A[0] += delta;
double fx2 = simulate(A);
A[0] -= delta;
printf("f(A) = %f, f(A + delta) = %f, f'(A)[0, 0] fd = %f\n", fx1, fx2,(fx2 - fx1) / delta);

__enzyme_autodiff<double>((void *)simulate, enzyme_dup, A, Adup);
printf("Adup[0, 0] = %f, Adup[0, 1] = %f", Adup[0], Adup[1]);
__enzyme_autodiff<double>((void *)simulate);

return 0;
}
2 changes: 1 addition & 1 deletion include/kalman/LinearizedMeasurementModel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ namespace Kalman {
//! Measurement vector type
using typename Base::Measurement;

protected:
public:
//! Measurement model jacobian
Jacobian<Measurement, State> H;
//! Measurement model noise jacobian
Expand Down