IQ.Lvbs/IQ.Pilot
8
7
Fork 3
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

48
iqpilot/selfdrive/state_estimation/native_binding_pyx.pyx Normal file
View File

@@ -0,0 +1,48 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import numpy as np
cimport numpy as np
cdef extern from "iqpilot/selfdrive/state_estimation/native_kernels.h":
void iq_estimator_car_predict(double *, double *, const double *, double, const double *)
void iq_estimator_car_update(double *, double *, int, const double *, const double *, double *)
void iq_estimator_pose_predict(double *, double *, const double *, double)
void iq_estimator_pose_update(double *, double *, int, const double *, const double *, double *)
def car_predict(np.ndarray[np.float64_t, ndim=1, mode="c"] state,
np.ndarray[np.float64_t, ndim=2, mode="c"] covariance,
np.ndarray[np.float64_t, ndim=2, mode="c"] process_noise,
double dt,
np.ndarray[np.float64_t, ndim=1, mode="c"] parameters):
iq_estimator_car_predict(&state[0], &covariance[0, 0], &process_noise[0, 0], dt, &parameters[0])
def car_update(np.ndarray[np.float64_t, ndim=1, mode="c"] state,
np.ndarray[np.float64_t, ndim=2, mode="c"] covariance,
int kind,
np.ndarray[np.float64_t, ndim=1, mode="c"] measurement,
np.ndarray[np.float64_t, ndim=2, mode="c"] noise):
cdef np.ndarray[np.float64_t, ndim=1, mode="c"] innovation = np.empty(measurement.size)
iq_estimator_car_update(&state[0], &covariance[0, 0], kind, &measurement[0], &noise[0, 0], &innovation[0])
return innovation
def pose_predict(np.ndarray[np.float64_t, ndim=1, mode="c"] state,
np.ndarray[np.float64_t, ndim=2, mode="c"] covariance,
np.ndarray[np.float64_t, ndim=2, mode="c"] process_noise,
double dt):
iq_estimator_pose_predict(&state[0], &covariance[0, 0], &process_noise[0, 0], dt)
def pose_update(np.ndarray[np.float64_t, ndim=1, mode="c"] state,
np.ndarray[np.float64_t, ndim=2, mode="c"] covariance,
int kind,
np.ndarray[np.float64_t, ndim=1, mode="c"] measurement,
np.ndarray[np.float64_t, ndim=2, mode="c"] noise):
cdef np.ndarray[np.float64_t, ndim=1, mode="c"] innovation = np.empty(measurement.size)
iq_estimator_pose_update(&state[0], &covariance[0, 0], kind, &measurement[0], &noise[0, 0], &innovation[0])
return innovation