forked from IQ.Lvbs/IQ.Pilot
49 lines
2.3 KiB
Cython
49 lines
2.3 KiB
Cython
"""
|
|
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, ¶meters[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
|