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

IQ.Pilot Release Commit @ cd83f5a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-08-23 11:32:03 -05:00
parent 58039e647c
commit a80e124cb8
116 changed files with 1657 additions and 4066 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
iqpilot/selfdrive/iqlocd/SConscript
View File

@@ -1,37 +1,8 @@
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
Import('env', 'arch', 'common', 'messaging', 'transformations')
loc_libs = [messaging, common, 'pthread', 'dl']
# build ekf models
rednose_gen_dir = 'models/generated'
rednose_gen_deps = [
"models/constants.py",
]
orbit_filter = env.RednoseCompileFilter(
target='orbit',
filter_gen_script='models/orbit_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=['orbit_state_constants.h'],
gen_script_deps=rednose_gen_deps,
)
car_ekf = env.RednoseCompileFilter(
target='car',
filter_gen_script='models/car_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=[],
gen_script_deps=rednose_gen_deps,
)
# iqlocd build
iqlocd_sources = ["atlas_loc_core.cc", "models/orbit_kf.cc"]
lenv = env.Clone()
# ekf filter libraries need to be linked, even if no symbols are used
if arch != "Darwin":
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
iqlocd = lenv.Program("iqlocd", iqlocd_sources, LIBS=["orbit", rednose] + loc_libs + transformations)
lenv.Depends(iqlocd, rednose)
lenv.Depends(iqlocd, orbit_filter)
iqlocd = lenv.Program("iqlocd", iqlocd_sources, LIBS=loc_libs + transformations)

1
iqpilot/selfdrive/iqlocd/atlas_loc_core.cc
View File

@@ -7,7 +7,6 @@
#include <cmath>
#include <vector>
using namespace EKFS;
using namespace Eigen;
ExitHandler do_exit;

222
iqpilot/selfdrive/iqlocd/models/car_kf.py Executable file → Normal file
View File

@@ -1,75 +1,63 @@
#!/usr/bin/env python3
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
import sys
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.common.swaglog import cloudlog
from rednose.helpers.kalmanfilter import KalmanFilter
if __name__ == '__main__': # Generating sympy
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
i = 0
def _slice(n):
global i
s = slice(i, i + n)
i += n
return s
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:
# Vehicle model params
STIFFNESS = _slice(1) # [-]
STEER_RATIO = _slice(1) # [-]
ANGLE_OFFSET = _slice(1) # [rad]
ANGLE_OFFSET_FAST = _slice(1) # [rad]
VELOCITY = _slice(2) # (x, y) [m/s]
YAW_RATE = _slice(1) # [rad/s]
STEER_ANGLE = _slice(1) # [rad]
ROAD_ROLL = _slice(1) # [rad]
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)
class CarKalman(KalmanFilter):
name = 'car'
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
initial_x = np.array([
1.0,
15.0,
0.0,
0.0,
10.0, 0.0,
0.0,
0.0,
0.0
])
# process noise
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,
])
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),
@@ -79,102 +67,28 @@ class CarKalman(KalmanFilter):
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
global_vars = [
'mass',
'rotational_inertia',
'center_to_front',
'center_to_rear',
'stiffness_front',
'stiffness_rear',
]
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)
@staticmethod
def generate_code(generated_dir):
dim_state = CarKalman.initial_x.shape[0]
name = CarKalman.name
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))
# Linearized single-track lateral dynamics, equations 7.211-7.213
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
# globals
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
m, j, aF, aR, cF_orig, cR_orig = global_vars
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
sf = state[States.STIFFNESS, :][0, 0]
cF, cR = sf * cF_orig, sf * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
theta = state[States.ROAD_ROLL, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
C = sp.Matrix(np.zeros((2, 1)))
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
C[1, 0] = 0
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir):
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
self.filter.set_global("mass", mass)
self.filter.set_global("rotational_inertia", rotational_inertia)
self.filter.set_global("center_to_front", center_to_front)
self.filter.set_global("center_to_rear", center_to_rear)
self.filter.set_global("stiffness_front", stiffness_front)
self.filter.set_global("stiffness_rear", stiffness_rear)
if __name__ == "__main__":
generated_dir = sys.argv[2]
CarKalman.generate_code(generated_dir)
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)

4
iqpilot/selfdrive/iqlocd/models/constants.py
View File

@@ -1,7 +1,3 @@
import os
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1

281
iqpilot/selfdrive/iqlocd/models/orbit_kf.cc
View File

@@ -1,122 +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"
using namespace EKFS;
using namespace Eigen;
#include <cmath>
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
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();
}
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
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;
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
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();
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> res;
for (const MatrixXdr &mat : mat_vec) {
res.push_back(get_mapmat(mat));
}
return res;
}
OrbitKalman::OrbitKalman() {
this->dim_state = orbit_initial_x.rows();
this->dim_state_err = orbit_initial_P_diag.rows();
this->initial_x = orbit_initial_x;
this->initial_P = orbit_initial_P_diag.asDiagonal();
this->fake_gps_pos_cov = orbit_fake_gps_pos_cov_diag.asDiagonal();
this->fake_gps_vel_cov = orbit_fake_gps_vel_cov_diag.asDiagonal();
this->reset_orientation_P = orbit_reset_orientation_diag.asDiagonal();
this->Q = orbit_Q_diag.asDiagonal();
for (auto& pair : orbit_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
}
// init filter
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
std::vector<int>{3}, std::vector<std::string>(), 0.8);
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) {
MatrixXdr covs = covs_diag.asDiagonal();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
filter->init_state(state, covs_diag.asDiagonal(), filter_time);
}
void OrbitKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
filter->init_state(state, covs, filter_time);
}
void OrbitKalman::init_state(const VectorXd &state, double filter_time) {
MatrixXdr covs = this->filter->covs();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
filter->init_state(state, filter->covariance(), filter_time);
}
VectorXd OrbitKalman::get_x() {
return this->filter->state();
}
MatrixXdr OrbitKalman::get_P() {
return this->filter->covs();
}
double OrbitKalman::get_filter_time() {
return this->filter->get_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) {
std::vector<MatrixXdr> R;
for (int i = 0; i < n; i++) {
R.push_back(this->obs_noise[kind]);
}
return R;
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) {
std::optional<Estimate> r;
if (R.size() == 0) {
R = this->get_R(kind, meas.size());
}
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
return r;
return filter->predict_and_observe(t, kind, meas, R);
}
void OrbitKalman::predict(double t) {
this->filter->predict(t);
}
const Eigen::VectorXd &OrbitKalman::get_initial_x() {
return this->initial_x;
}
const MatrixXdr &OrbitKalman::get_initial_P() {
return this->initial_P;
}
const MatrixXdr &OrbitKalman::get_fake_gps_pos_cov() {
return this->fake_gps_pos_cov;
}
const MatrixXdr &OrbitKalman::get_fake_gps_vel_cov() {
return this->fake_gps_vel_cov;
}
const MatrixXdr &OrbitKalman::get_reset_orientation_P() {
return this->reset_orientation_P;
}
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) {
assert(in.size() == 6);
Matrix<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
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;
}

38
iqpilot/selfdrive/iqlocd/models/orbit_kf.h
View File

@@ -1,66 +1,46 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#pragma once
#include <string>
#include <cmath>
#include <memory>
#include <optional>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include "generated/orbit_state_constants.h"
#include "rednose/helpers/ekf_sym.h"
#include "iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h"
#include "iqpilot/selfdrive/state_estimation/estimator.h"
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
using namespace EKFS;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
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 = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
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::string name = "orbit";
std::shared_ptr<EKFSym> filter;
int dim_state;
int dim_state_err;
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;
MatrixXdr Q; // process noise
std::unordered_map<int, MatrixXdr> obs_noise;
};

242
iqpilot/selfdrive/iqlocd/models/orbit_kf.py
View File

@@ -1,242 +0,0 @@
#!/usr/bin/env python3
import sys
import os
import numpy as np
from iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
import sympy as sp
import inspect
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
from rednose.helpers.ekf_sym import gen_code
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
def numpy2eigenstring(arr):
assert(len(arr.shape) == 1)
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
class States:
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
ACCELERATION_ERR = slice(15, 18)
ACC_BIAS_ERR = slice(18, 21)
class OrbitScopeModel:
name = 'orbit'
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
initial_P_diag = np.array([10**2, 10**2, 10**2,
0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
100**2, 100**2, 100**2,
0.01**2, 0.01**2, 0.01**2])
# state covariance when resetting midway in a segment
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
3**2, 3**2, 3**2,
0.005**2, 0.005**2, 0.005**2])
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
@staticmethod
def generate_code(generated_dir):
name = OrbitScopeModel.name
dim_state = OrbitScopeModel.initial_x.shape[0]
dim_state_err = OrbitScopeModel.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym]
#
# Observation functions
#
h_gyro_sym = sp.Matrix([
vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
# write constants to extra header file for use in cpp
orbit_header = "#pragma once\n\n"
orbit_header += "#include <unordered_map>\n"
orbit_header += "#include <eigen3/Eigen/Dense>\n\n"
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
assert(slc.step is None) # unsupported
orbit_header += f'#define STATE_{state}_START {slc.start}\n'
orbit_header += f'#define STATE_{state}_END {slc.stop}\n'
orbit_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
orbit_header += "\n"
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
orbit_header += f'#define OBSERVATION_{kind} {val}\n'
orbit_header += "\n"
orbit_header += f"static const Eigen::VectorXd orbit_initial_x = {numpy2eigenstring(OrbitScopeModel.initial_x)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_initial_P_diag = {numpy2eigenstring(OrbitScopeModel.initial_P_diag)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_fake_gps_pos_cov_diag = {numpy2eigenstring(OrbitScopeModel.fake_gps_pos_cov_diag)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_fake_gps_vel_cov_diag = {numpy2eigenstring(OrbitScopeModel.fake_gps_vel_cov_diag)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_reset_orientation_diag = {numpy2eigenstring(OrbitScopeModel.reset_orientation_diag)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_Q_diag = {numpy2eigenstring(OrbitScopeModel.Q_diag)};\n"
orbit_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> orbit_obs_noise_diag = {\n"
for kind, noise in OrbitScopeModel.obs_noise_diag.items():
orbit_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
orbit_header += "};\n\n"
open(os.path.join(generated_dir, "orbit_state_constants.h"), 'w').write(orbit_header)
if __name__ == "__main__":
generated_dir = sys.argv[2]
OrbitScopeModel.generate_code(generated_dir)

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