github-actions[bot]
de77c89cf6
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
43 lines
1.3 KiB
C++
43 lines
1.3 KiB
C++
#pragma once
|
|
|
|
#include <iostream>
|
|
#include <cassert>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <deque>
|
|
#include <unordered_map>
|
|
#include <map>
|
|
#include <cmath>
|
|
|
|
#include <eigen3/Eigen/Dense>
|
|
|
|
typedef void (*extra_routine_t)(double *, double *);
|
|
|
|
struct EKF {
|
|
std::string name;
|
|
std::vector<int> kinds;
|
|
std::vector<int> feature_kinds;
|
|
|
|
void (*f_fun)(double *, double, double *);
|
|
void (*F_fun)(double *, double, double *);
|
|
void (*err_fun)(double *, double *, double *);
|
|
void (*inv_err_fun)(double *, double *, double *);
|
|
void (*H_mod_fun)(double *, double *);
|
|
void (*predict)(double *, double *, double *, double);
|
|
std::unordered_map<int, void (*)(double *, double *, double *)> hs = {};
|
|
std::unordered_map<int, void (*)(double *, double *, double *)> Hs = {};
|
|
std::unordered_map<int, void (*)(double *, double *, double *, double *, double *)> updates = {};
|
|
std::unordered_map<int, void (*)(double *, double *, double *)> Hes = {};
|
|
std::unordered_map<std::string, void (*)(double)> sets = {};
|
|
std::unordered_map<std::string, extra_routine_t> extra_routines = {};
|
|
};
|
|
|
|
#define ekf_lib_init(ekf) \
|
|
extern "C" void* ekf_get() { \
|
|
return (void*) &ekf; \
|
|
} \
|
|
extern void __attribute__((weak)) ekf_register(const EKF* ptr); \
|
|
static void __attribute__((constructor)) do_ekf_init_ ## ekf(void) { \
|
|
if (ekf_register) ekf_register(&ekf); \
|
|
}
|