mirror of
https://github.com/commaai/rednose.git
synced 2026-06-14 08:56:25 +08:00
70 lines
1.4 KiB
Python
Executable File
70 lines
1.4 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
import sys
|
|
|
|
import numpy as np
|
|
import sympy as sp
|
|
|
|
from rednose import KalmanFilter
|
|
from rednose.helpers.ekf_sym import gen_code
|
|
|
|
|
|
class ObservationKind():
|
|
UNKNOWN = 0
|
|
NO_OBSERVATION = 1
|
|
POSITION = 1
|
|
|
|
names = [
|
|
'Unknown',
|
|
'No observation',
|
|
'Position'
|
|
]
|
|
|
|
@classmethod
|
|
def to_string(cls, kind):
|
|
return cls.names[kind]
|
|
|
|
|
|
class States():
|
|
POSITION = slice(0, 1)
|
|
VELOCITY = slice(1, 2)
|
|
|
|
|
|
class KinematicKalman(KalmanFilter):
|
|
name = 'kinematic'
|
|
|
|
initial_x = np.array([0.5, 0.0])
|
|
|
|
# state covariance
|
|
initial_P_diag = np.array([1.0**2, 1.0**2])
|
|
|
|
# process noise
|
|
Q = np.diag([0.1**2, 2.0**2])
|
|
|
|
obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)}
|
|
|
|
@staticmethod
|
|
def generate_code(generated_dir):
|
|
name = KinematicKalman.name
|
|
dim_state = KinematicKalman.initial_x.shape[0]
|
|
|
|
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
|
state = sp.Matrix(state_sym)
|
|
|
|
position = state[States.POSITION, :][0,:]
|
|
velocity = state[States.VELOCITY, :][0,:]
|
|
|
|
dt = sp.Symbol('dt')
|
|
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
|
state_dot[States.POSITION.start, 0] = velocity
|
|
f_sym = state + dt * state_dot
|
|
|
|
obs_eqs = [
|
|
[sp.Matrix([position]), ObservationKind.POSITION, None],
|
|
]
|
|
|
|
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
|
|
|
|
if __name__ == "__main__":
|
|
generated_dir = sys.argv[2]
|
|
KinematicKalman.generate_code(generated_dir)
|