IQ.Lvbs/IQ.Pilot
8
7
Fork 3
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ b6534c0

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-08-27 20:17:33 -05:00
commit 00f07cac48
4706 changed files with 1257146 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
iqpilot/selfdrive/iqlocd/models/__init__.py Normal file
View File

94
iqpilot/selfdrive/iqlocd/models/car_kf.py Normal file
View File

@@ -0,0 +1,94 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
from typing import Any
import numpy as np
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import car_predict, car_update
except ModuleNotFoundError:
car_predict = None
car_update = None
class States:
STIFFNESS = slice(0, 1)
STEER_RATIO = slice(1, 2)
ANGLE_OFFSET = slice(2, 3)
ANGLE_OFFSET_FAST = slice(3, 4)
VELOCITY = slice(4, 6)
YAW_RATE = slice(6, 7)
STEER_ANGLE = slice(7, 8)
ROAD_ROLL = slice(8, 9)
def _transition(state: np.ndarray, dt: float, values: dict[str, float]) -> np.ndarray:
result = state.copy()
stiffness = state[0]
steer_ratio = state[1]
angle = state[7] - state[2] - state[3]
speed, lateral_speed = state[4:6]
yaw_rate = state[6]
mass = values["mass"]
inertia = values["rotational_inertia"]
front = values["center_to_front"]
rear = values["center_to_rear"]
front_stiffness = stiffness * values["stiffness_front"]
rear_stiffness = stiffness * values["stiffness_rear"]
lateral_dot = -(front_stiffness + rear_stiffness) * lateral_speed / (mass * speed)
lateral_dot += (-(front_stiffness * front - rear_stiffness * rear) / (mass * speed) - speed) * yaw_rate
lateral_dot += front_stiffness * angle / (mass * steer_ratio) - ACCELERATION_DUE_TO_GRAVITY * state[8]
yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed)
yaw_dot -= (front_stiffness * front**2 + rear_stiffness * rear**2) * yaw_rate / (inertia * speed)
yaw_dot += front_stiffness * front * angle / (inertia * steer_ratio)
result[5] += dt * lateral_dot
result[6] += dt * yaw_dot
return result
class CarKalman(EstimatorModel):
name = "car"
initial_x = np.array([1.0, 15.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0])
Q = np.diag([(.05 / 100)**2, .01**2, math.radians(0.02)**2, math.radians(0.25)**2,
.1**2, .01**2, math.radians(0.1)**2, math.radians(0.1)**2, math.radians(1)**2])
P_initial = Q.copy()
obs_noise: dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
def __init__(self):
self.native_parameters = np.zeros(6)
measurements = {
ObservationKind.ROAD_FRAME_YAW_RATE: lambda state, _: state[6:7],
ObservationKind.ROAD_FRAME_XY_SPEED: lambda state, _: state[4:6],
ObservationKind.ROAD_FRAME_X_SPEED: lambda state, _: state[4:5],
ObservationKind.STEER_ANGLE: lambda state, _: state[7:8],
ObservationKind.ANGLE_OFFSET_FAST: lambda state, _: state[3:4],
ObservationKind.STEER_RATIO: lambda state, _: state[1:2],
ObservationKind.STIFFNESS: lambda state, _: state[0:1],
ObservationKind.ROAD_ROLL: lambda state, _: state[8:9],
}
def native_predict(state, covariance, dt, process_noise, _):
car_predict(state, covariance, process_noise, dt, self.native_parameters)
model = ModelDefinition(9, 9, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if car_predict is not None else None, native_update=car_update)
super().__init__(StateEstimator(model, self.initial_x, self.P_initial, max_rewind_age=0.8))
def set_globals(self, mass: float, rotational_inertia: float, center_to_front: float, center_to_rear: float,
stiffness_front: float, stiffness_rear: float) -> None:
self.native_parameters[:] = mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear
for name, value in locals().copy().items():
if name != "self":
self.filter.set_global(name, value)

88
iqpilot/selfdrive/iqlocd/models/constants.py Normal file
View File

@@ -0,0 +1,88 @@
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 35
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ORB_FEATURES_WIDE = 34
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
ROAD_ROLL = 31 # [rad]
names = [
'Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
'pseudorange',
'pseudorange rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
'Steer Angle',
'Fast Angle Offset',
'Stiffness',
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
'ECEF orientation from GPS',
'NO accel',
'ORB features wide camera',
'ECEF_VEL',
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
ObservationKind.PSEUDORANGE_RATE_GPS,
ObservationKind.PSEUDORANGE_GLONASS,
ObservationKind.PSEUDORANGE_RATE_GLONASS]

225
iqpilot/selfdrive/iqlocd/models/orbit_kf.cc Normal file
View File

@@ -0,0 +1,225 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#include "iqpilot/selfdrive/iqlocd/models/orbit_kf.h"
#include <cmath>
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;
using Eigen::VectorXd;
using iqpilot::state_estimation::ModelDefinition;
using iqpilot::state_estimation::StateEstimator;
namespace {
constexpr double EARTH_GM = 3.986005e14;
Matrix3d rotation(const VectorXd &state) {
return Quaterniond(state(3), state(4), state(5), state(6)).normalized().toRotationMatrix();
}
Matrix3d skew(const Vector3d &value) {
Matrix3d result;
result << 0.0, -value.z(), value.y(), value.z(), 0.0, -value.x(), -value.y(), value.x(), 0.0;
return result;
}
VectorXd transition(const VectorXd &state, double dt) {
VectorXd result = state;
const Quaterniond orientation(state(3), state(4), state(5), state(6));
const Vector3d omega = state.segment<3>(10);
const Quaterniond derivative(0.0, omega.x(), omega.y(), omega.z());
const Quaterniond rate = orientation * derivative;
result.segment<3>(0) += dt * state.segment<3>(7);
result.segment<4>(3) += 0.5 * dt * (VectorXd(4) << rate.w(), rate.x(), rate.y(), rate.z()).finished();
result.segment<3>(7) += dt * rotation(state) * state.segment<3>(16);
return result;
}
VectorXd normalize(const VectorXd &state) {
VectorXd result = state;
result.segment<4>(3) /= result.segment<4>(3).norm();
return result;
}
VectorXd inject(const VectorXd &state, const VectorXd &delta) {
VectorXd result = state;
result.segment<3>(0) += delta.segment<3>(0);
const Quaterniond orientation(state(3), state(4), state(5), state(6));
Quaterniond error(1.0, 0.5 * delta(3), 0.5 * delta(4), 0.5 * delta(5));
const Quaterniond updated = error * orientation;
result.segment<4>(3) << updated.w(), updated.x(), updated.y(), updated.z();
result.segment(7, 15) += delta.segment(6, 15);
return normalize(result);
}
MatrixXdr error_projection(const VectorXd &state) {
MatrixXdr projection = MatrixXdr::Zero(22, 21);
projection.block<3, 3>(0, 0).setIdentity();
const double w = state(3);
const double x = state(4);
const double y = state(5);
const double z = state(6);
projection.block<4, 3>(3, 3) << -0.5 * x, -0.5 * y, -0.5 * z,
0.5 * w, 0.5 * z, -0.5 * y,
-0.5 * z, 0.5 * w, 0.5 * x,
0.5 * y, -0.5 * x, 0.5 * w;
projection.block(7, 6, 15, 15).setIdentity();
return projection;
}
MatrixXdr orbit_error_transition(const VectorXd &state, double dt) {
MatrixXdr result = MatrixXdr::Identity(21, 21);
const Matrix3d transform = rotation(state);
result.block<3, 3>(0, 6) = Matrix3d::Identity() * dt;
result.block<3, 3>(3, 3) += -dt * skew(transform * state.segment<3>(10));
result.block<3, 3>(3, 9) = dt * transform;
result.block<3, 3>(6, 3) = -dt * skew(transform * state.segment<3>(16));
result.block<3, 3>(6, 15) = dt * transform;
return result;
}
MatrixXdr selected_jacobian(int start) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
result.block<3, 3>(0, start).setIdentity();
return result;
}
VectorXd phone_acceleration(const VectorXd &state) {
const Vector3d position = state.segment<3>(0);
const Vector3d gravity = rotation(state).transpose() * (EARTH_GM * position / std::pow(position.squaredNorm(), 1.5));
return gravity + state.segment<3>(16) + state.segment<3>(19);
}
MatrixXdr diagonal(std::initializer_list<double> values) {
VectorXd vector(values.size());
int index = 0;
for (double value : values) vector(index++) = value;
return vector.asDiagonal();
}
}
OrbitKalman::OrbitKalman() {
initial_x.resize(22);
initial_x << 3.88e6, -3.37e6, 3.76e6, 0.42254641, -0.31238054, -0.83602975, -0.15788347,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
initial_P = diagonal({100.0, 100.0, 100.0, 0.0001, 0.0001, 0.0001, 100.0, 100.0, 100.0,
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10000.0, 10000.0, 10000.0, 0.0001, 0.0001, 0.0001});
fake_gps_pos_cov = diagonal({1e6, 1e6, 1e6});
fake_gps_vel_cov = diagonal({100.0, 100.0, 100.0});
reset_orientation_P = diagonal({1.0, 1.0, 1.0});
obs_noise = {
{OBSERVATION_PHONE_GYRO, diagonal({0.000625, 0.000625, 0.000625})},
{OBSERVATION_PHONE_ACCEL, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_CAMERA_ODO_ROTATION, diagonal({0.0025, 0.0025, 0.0025})},
{OBSERVATION_CAMERA_ODO_TRANSLATION, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_NO_ROT, diagonal({0.000025, 0.000025, 0.000025})},
{OBSERVATION_NO_ACCEL, diagonal({0.0025, 0.0025, 0.0025})},
{OBSERVATION_ECEF_POS, diagonal({25.0, 25.0, 25.0})},
{OBSERVATION_ECEF_VEL, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, diagonal({0.04, 0.04, 0.04, 0.04})},
};
const MatrixXdr process_noise = diagonal({0.0009, 0.0009, 0.0009, 0.000001, 0.000001, 0.000001,
0.0001, 0.0001, 0.0001, 0.01, 0.01, 0.01,
2.5e-9, 2.5e-9, 2.5e-9, 9.0, 9.0, 9.0, 0.000025, 0.000025, 0.000025});
std::unordered_map<int, std::function<VectorXd(const VectorXd &)>> measurements = {
{OBSERVATION_PHONE_GYRO, [](const VectorXd &state) { return state.segment<3>(10) + state.segment<3>(13); }},
{OBSERVATION_NO_ROT, [](const VectorXd &state) { return state.segment<3>(10); }},
{OBSERVATION_PHONE_ACCEL, phone_acceleration},
{OBSERVATION_ECEF_POS, [](const VectorXd &state) { return state.segment<3>(0); }},
{OBSERVATION_ECEF_VEL, [](const VectorXd &state) { return state.segment<3>(7); }},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, [](const VectorXd &state) { return state.segment<4>(3); }},
{OBSERVATION_CAMERA_ODO_TRANSLATION, [](const VectorXd &state) { return rotation(state).transpose() * state.segment<3>(7); }},
{OBSERVATION_CAMERA_ODO_ROTATION, [](const VectorXd &state) { return state.segment<3>(10); }},
{OBSERVATION_NO_ACCEL, [](const VectorXd &state) { return state.segment<3>(16); }},
};
std::unordered_map<int, std::function<MatrixXdr(const VectorXd &)>> observation_jacobians = {
{OBSERVATION_PHONE_GYRO, [](const VectorXd &) {
MatrixXdr result = selected_jacobian(9);
result.block<3, 3>(0, 12).setIdentity();
return result;
}},
{OBSERVATION_NO_ROT, [](const VectorXd &) { return selected_jacobian(9); }},
{OBSERVATION_PHONE_ACCEL, [](const VectorXd &state) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
const Vector3d position = state.segment<3>(0);
const double radius_squared = position.squaredNorm();
const double radius = std::sqrt(radius_squared);
const Vector3d gravity = EARTH_GM * position / (radius_squared * radius);
result.block<3, 3>(0, 0) = rotation(state).transpose() * EARTH_GM *
(Matrix3d::Identity() / (radius_squared * radius) -
3.0 * position * position.transpose() / (radius_squared * radius_squared * radius));
result.block<3, 3>(0, 3) = rotation(state).transpose() * skew(gravity);
result.block<3, 3>(0, 15).setIdentity();
result.block<3, 3>(0, 18).setIdentity();
return result;
}},
{OBSERVATION_ECEF_POS, [](const VectorXd &) { return selected_jacobian(0); }},
{OBSERVATION_ECEF_VEL, [](const VectorXd &) { return selected_jacobian(6); }},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, [](const VectorXd &state) { return error_projection(state).block(3, 0, 4, 21); }},
{OBSERVATION_CAMERA_ODO_TRANSLATION, [](const VectorXd &state) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
result.block<3, 3>(0, 3) = rotation(state).transpose() * skew(state.segment<3>(7));
result.block<3, 3>(0, 6) = rotation(state).transpose();
return result;
}},
{OBSERVATION_CAMERA_ODO_ROTATION, [](const VectorXd &) { return selected_jacobian(9); }},
{OBSERVATION_NO_ACCEL, [](const VectorXd &) { return selected_jacobian(15); }},
};
ModelDefinition model{22, 21, transition, measurements, process_noise, obs_noise, inject, error_projection, normalize,
orbit_error_transition, observation_jacobians};
filter = std::make_shared<StateEstimator>(std::move(model), initial_x, initial_P);
}
void OrbitKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
filter->init_state(state, covs_diag.asDiagonal(), filter_time);
}
void OrbitKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
filter->init_state(state, covs, filter_time);
}
void OrbitKalman::init_state(const VectorXd &state, double filter_time) {
filter->init_state(state, filter->covariance(), filter_time);
}
VectorXd OrbitKalman::get_x() { return filter->state(); }
MatrixXdr OrbitKalman::get_P() { return filter->covariance(); }
double OrbitKalman::get_filter_time() { return filter->time(); }
std::vector<MatrixXdr> OrbitKalman::get_R(int kind, int n) {
return std::vector<MatrixXdr>(n, obs_noise.at(kind));
}
std::optional<Estimate> OrbitKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
return filter->predict_and_observe(t, kind, meas, R);
}
void OrbitKalman::predict(double t) { filter->predict(t); }
const VectorXd &OrbitKalman::get_initial_x() { return initial_x; }
const MatrixXdr &OrbitKalman::get_initial_P() { return initial_P; }
const MatrixXdr &OrbitKalman::get_fake_gps_pos_cov() { return fake_gps_pos_cov; }
const MatrixXdr &OrbitKalman::get_fake_gps_vel_cov() { return fake_gps_vel_cov; }
const MatrixXdr &OrbitKalman::get_reset_orientation_P() { return reset_orientation_P; }
MatrixXdr OrbitKalman::H(const VectorXd &in) {
if (in.size() != 6) throw std::invalid_argument("local velocity input dimension mismatch");
auto function = [](const VectorXd &value) {
const Matrix3d transform = (Eigen::AngleAxisd(value(2), Vector3d::UnitZ()) * Eigen::AngleAxisd(value(1), Vector3d::UnitY()) *
Eigen::AngleAxisd(value(0), Vector3d::UnitX())).toRotationMatrix();
return transform.transpose() * value.segment<3>(3);
};
MatrixXdr result(3, 6);
for (int index = 0; index < 6; ++index) {
const double step = std::cbrt(Eigen::NumTraits<double>::epsilon()) * std::max(1.0, std::abs(in(index)));
VectorXd upper = in;
VectorXd lower = in;
upper(index) += step;
lower(index) -= step;
result.col(index) = (function(upper) - function(lower)) / (2.0 * step);
}
return result;
}

46
iqpilot/selfdrive/iqlocd/models/orbit_kf.h Normal file
View File

@@ -0,0 +1,46 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#pragma once
#include <memory>
#include <optional>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Dense>
#include "iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h"
#include "iqpilot/selfdrive/state_estimation/estimator.h"
using MatrixXdr = iqpilot::state_estimation::Matrix;
using Estimate = iqpilot::state_estimation::Estimate;
class OrbitKalman {
public:
OrbitKalman();
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
void init_state(const Eigen::VectorXd &state, double filter_time);
Eigen::VectorXd get_x();
MatrixXdr get_P();
double get_filter_time();
std::vector<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
void predict(double t);
const Eigen::VectorXd &get_initial_x();
const MatrixXdr &get_initial_P();
const MatrixXdr &get_fake_gps_pos_cov();
const MatrixXdr &get_fake_gps_vel_cov();
const MatrixXdr &get_reset_orientation_P();
MatrixXdr H(const Eigen::VectorXd &in);
private:
std::shared_ptr<iqpilot::state_estimation::StateEstimator> filter;
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
MatrixXdr fake_gps_pos_cov;
MatrixXdr fake_gps_vel_cov;
MatrixXdr reset_orientation_P;
std::unordered_map<int, MatrixXdr> obs_noise;
};

42
iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h Normal file
View File

@@ -0,0 +1,42 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#pragma once
#define STATE_ECEF_POS_START 0
#define STATE_ECEF_POS_LEN 3
#define STATE_ECEF_ORIENTATION_START 3
#define STATE_ECEF_ORIENTATION_LEN 4
#define STATE_ECEF_VELOCITY_START 7
#define STATE_ECEF_VELOCITY_LEN 3
#define STATE_ANGULAR_VELOCITY_START 10
#define STATE_ANGULAR_VELOCITY_LEN 3
#define STATE_GYRO_BIAS_START 13
#define STATE_GYRO_BIAS_LEN 3
#define STATE_ACCELERATION_START 16
#define STATE_ACCELERATION_LEN 3
#define STATE_ACC_BIAS_START 19
#define STATE_ACC_BIAS_LEN 3
#define STATE_ECEF_POS_ERR_START 0
#define STATE_ECEF_POS_ERR_LEN 3
#define STATE_ECEF_ORIENTATION_ERR_START 3
#define STATE_ECEF_ORIENTATION_ERR_LEN 3
#define STATE_ECEF_VELOCITY_ERR_START 6
#define STATE_ECEF_VELOCITY_ERR_LEN 3
#define STATE_ANGULAR_VELOCITY_ERR_START 9
#define STATE_ANGULAR_VELOCITY_ERR_LEN 3
#define STATE_GYRO_BIAS_ERR_START 12
#define STATE_GYRO_BIAS_ERR_LEN 3
#define STATE_ACCELERATION_ERR_START 15
#define STATE_ACCELERATION_ERR_LEN 3
#define STATE_ACC_BIAS_ERR_START 18
#define STATE_ACC_BIAS_ERR_LEN 3
#define OBSERVATION_PHONE_GYRO 4
#define OBSERVATION_NO_ROT 9
#define OBSERVATION_PHONE_ACCEL 10
#define OBSERVATION_ECEF_POS 12
#define OBSERVATION_CAMERA_ODO_TRANSLATION 13
#define OBSERVATION_CAMERA_ODO_ROTATION 14
#define OBSERVATION_ECEF_ORIENTATION_FROM_GPS 32
#define OBSERVATION_NO_ACCEL 33
#define OBSERVATION_ECEF_VEL 35