<commit_before>#!/usr/bin/env python3

import sys

import numpy as np
import sympy as sp

from selfdrive.locationd.models.constants import ObservationKind
from rednose.helpers.ekf_sym import EKF_sym, gen_code
from rednose.helpers.lst_sq_computer import LstSqComputer
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate

EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)


def parse_prr(m):
  from laika.raw_gnss import GNSSMeasurement
  sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
                                  m[GNSSMeasurement.SAT_VEL]))
  R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
  z_i = m[GNSSMeasurement.PRR]
  return z_i, R_i, sat_pos_vel_i


def parse_pr(m):
  from laika.raw_gnss import GNSSMeasurement
  pseudorange = m[GNSSMeasurement.PR]
  pseudorange_stdev = m[GNSSMeasurement.PR_STD]
  sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
                                   np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
  z_i = np.atleast_1d(pseudorange)
  R_i = np.atleast_2d(pseudorange_stdev**2)
  return z_i, R_i, sat_pos_freq_i


class States():
  ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
  ECEF_ORIENTATION = slice(3, 7)  # quat for orientation 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
  CLOCK_BIAS = slice(13, 14)  # clock bias in light-meters,
  CLOCK_DRIFT = slice(14, 15)  # clock drift in light-meters/s,
  GYRO_BIAS = slice(15, 18)  # roll, pitch and yaw biases
  ODO_SCALE = slice(18, 19)  # odometer scale
  ACCELERATION = slice(19, 22)  # Acceleration in device frame in m/s**2
  FOCAL_SCALE = slice(22, 23)  # focal length scale
  IMU_OFFSET = slice(23, 26)  # imu offset angles in radians
  GLONASS_BIAS = slice(26, 27)  # GLONASS bias in m expressed as bias + freq_num*freq_slope
  GLONASS_FREQ_SLOPE = slice(27, 28)  # GLONASS bias in m expressed as bias + freq_num*freq_slope
  CLOCK_ACCELERATION = slice(28, 29)  # clock acceleration in light-meters/s**2,
  ACCELEROMETER_SCALE = slice(29, 30)  # scale of mems accelerometer
  ACCELEROMETER_BIAS = slice(30, 33)  # bias of mems accelerometer

  # 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)
  CLOCK_BIAS_ERR = slice(12, 13)
  CLOCK_DRIFT_ERR = slice(13, 14)
  GYRO_BIAS_ERR = slice(14, 17)
  ODO_SCALE_ERR = slice(17, 18)
  ACCELERATION_ERR = slice(18, 21)
  FOCAL_SCALE_ERR = slice(21, 22)
  IMU_OFFSET_ERR = slice(22, 25)
  GLONASS_BIAS_ERR = slice(25, 26)
  GLONASS_FREQ_SLOPE_ERR = slice(26, 27)
  CLOCK_ACCELERATION_ERR = slice(27, 28)
  ACCELEROMETER_SCALE_ERR = slice(28, 29)
  ACCELEROMETER_BIAS_ERR = slice(29, 32)


class LocKalman():
  name = "loc"
  x_initial = np.array([0, 0, 0,
                        1, 0, 0, 0,
                        0, 0, 0,
                        0, 0, 0,
                        0, 0,
                        0, 0, 0,
                        1,
                        0, 0, 0,
                        1,
                        0, 0, 0,
                        0, 0,
                        0,
                        1,
                        0, 0, 0], dtype=np.float64)

  # state covariance
  P_initial = np.diag([1e16, 1e16, 1e16,
                       10**2, 10**2, 10**2,
                       10**2, 10**2, 10**2,
                       1**2, 1**2, 1**2,
                       1e14, (100)**2,
                       0.05**2, 0.05**2, 0.05**2,
                       0.02**2,
                       2**2, 2**2, 2**2,
                       0.01**2,
                       (0.01)**2, (0.01)**2, (0.01)**2,
                       10**2, 1**2,
                       0.2**2,
                       0.05**2,
                       0.05**2, 0.05**2, 0.05**2])

  # process noise
  Q = np.diag([0.03**2, 0.03**2, 0.03**2,
               0.0**2, 0.0**2, 0.0**2,
               0.0**2, 0.0**2, 0.0**2,
               0.1**2, 0.1**2, 0.1**2,
               (.1)**2, (0.0)**2,
               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
               (0.02 / 100)**2,
               3**2, 3**2, 3**2,
               0.001**2,
               (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
               (.1)**2, (.01)**2,
               0.005**2,
               (0.02 / 100)**2,
               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2])

  # measurements that need to pass mahalanobis distance outlier rejector
  maha_test_kinds = [ObservationKind.ORB_FEATURES]  # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
  dim_augment = 7
  dim_augment_err = 6

  @staticmethod
  def generate_code(generated_dir, N=4):
    dim_augment = LocKalman.dim_augment
    dim_augment_err = LocKalman.dim_augment_err

    dim_main = LocKalman.x_initial.shape[0]
    dim_main_err = LocKalman.P_initial.shape[0]
    dim_state = dim_main + dim_augment * N
    dim_state_err = dim_main_err + dim_augment_err * N
    maha_test_kinds = LocKalman.maha_test_kinds

    name = f"{LocKalman.name}_{N}"

    # make functions and jacobians with sympy
    # state variables
    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
    cb = state[States.CLOCK_BIAS, :]
    cd = state[States.CLOCK_DRIFT, :]
    roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
    odo_scale = state[States.ODO_SCALE, :]
    acceleration = state[States.ACCELERATION, :]
    focal_scale = state[States.FOCAL_SCALE, :]
    imu_angles = state[States.IMU_OFFSET, :]
    imu_angles[0, 0] = 0
    imu_angles[2, 0] = 0
    glonass_bias = state[States.GLONASS_BIAS, :]
    glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :]
    ca = state[States.CLOCK_ACCELERATION, :]
    # accel_scale = state[States.ACCELEROMETER_SCALE, :]
    accel_bias = state[States.ACCELEROMETER_BIAS, :]<commit_msg>add a note in loc_kf (#23082)<commit_after>#!/usr/bin/env python3

import sys

import numpy as np
import sympy as sp

from selfdrive.locationd.models.constants import ObservationKind
from rednose.helpers.ekf_sym import EKF_sym, gen_code
from rednose.helpers.lst_sq_computer import LstSqComputer
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate

EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)


def parse_prr(m):
  from laika.raw_gnss import GNSSMeasurement
  sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
                                  m[GNSSMeasurement.SAT_VEL]))
  R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
  z_i = m[GNSSMeasurement.PRR]
  return z_i, R_i, sat_pos_vel_i


def parse_pr(m):
  from laika.raw_gnss import GNSSMeasurement
  pseudorange = m[GNSSMeasurement.PR]
  pseudorange_stdev = m[GNSSMeasurement.PR_STD]
  sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
                                   np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
  z_i = np.atleast_1d(pseudorange)
  R_i = np.atleast_2d(pseudorange_stdev**2)
  return z_i, R_i, sat_pos_freq_i


class States():
  ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
  ECEF_ORIENTATION = slice(3, 7)  # quat for orientation 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
  CLOCK_BIAS = slice(13, 14)  # clock bias in light-meters,
  CLOCK_DRIFT = slice(14, 15)  # clock drift in light-meters/s,
  GYRO_BIAS = slice(15, 18)  # roll, pitch and yaw biases
  ODO_SCALE = slice(18, 19)  # odometer scale
  ACCELERATION = slice(19, 22)  # Acceleration in device frame in m/s**2
  FOCAL_SCALE = slice(22, 23)  # focal length scale
  IMU_OFFSET = slice(23, 26)  # imu offset angles in radians
  GLONASS_BIAS = slice(26, 27)  # GLONASS bias in m expressed as bias + freq_num*freq_slope
  GLONASS_FREQ_SLOPE = slice(27, 28)  # GLONASS bias in m expressed as bias + freq_num*freq_slope
  CLOCK_ACCELERATION = slice(28, 29)  # clock acceleration in light-meters/s**2,
  ACCELEROMETER_SCALE = slice(29, 30)  # scale of mems accelerometer
  ACCELEROMETER_BIAS = slice(30, 33)  # bias of mems accelerometer
  # We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_OFFSET).
  # From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE

  # 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)
  CLOCK_BIAS_ERR = slice(12, 13)
  CLOCK_DRIFT_ERR = slice(13, 14)
  GYRO_BIAS_ERR = slice(14, 17)
  ODO_SCALE_ERR = slice(17, 18)
  ACCELERATION_ERR = slice(18, 21)
  FOCAL_SCALE_ERR = slice(21, 22)
  IMU_OFFSET_ERR = slice(22, 25)
  GLONASS_BIAS_ERR = slice(25, 26)
  GLONASS_FREQ_SLOPE_ERR = slice(26, 27)
  CLOCK_ACCELERATION_ERR = slice(27, 28)
  ACCELEROMETER_SCALE_ERR = slice(28, 29)
  ACCELEROMETER_BIAS_ERR = slice(29, 32)


class LocKalman():
  name = "loc"
  x_initial = np.array([0, 0, 0,
                        1, 0, 0, 0,
                        0, 0, 0,
                        0, 0, 0,
                        0, 0,
                        0, 0, 0,
                        1,
                        0, 0, 0,
                        1,
                        0, 0, 0,
                        0, 0,
                        0,
                        1,
                        0, 0, 0], dtype=np.float64)

  # state covariance
  P_initial = np.diag([1e16, 1e16, 1e16,
                       10**2, 10**2, 10**2,
                       10**2, 10**2, 10**2,
                       1**2, 1**2, 1**2,
                       1e14, (100)**2,
                       0.05**2, 0.05**2, 0.05**2,
                       0.02**2,
                       2**2, 2**2, 2**2,
                       0.01**2,
                       (0.01)**2, (0.01)**2, (0.01)**2,
                       10**2, 1**2,
                       0.2**2,
                       0.05**2,
                       0.05**2, 0.05**2, 0.05**2])

  # process noise
  Q = np.diag([0.03**2, 0.03**2, 0.03**2,
               0.0**2, 0.0**2, 0.0**2,
               0.0**2, 0.0**2, 0.0**2,
               0.1**2, 0.1**2, 0.1**2,
               (.1)**2, (0.0)**2,
               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
               (0.02 / 100)**2,
               3**2, 3**2, 3**2,
               0.001**2,
               (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
               (.1)**2, (.01)**2,
               0.005**2,
               (0.02 / 100)**2,
               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2])

  # measurements that need to pass mahalanobis distance outlier rejector
  maha_test_kinds = [ObservationKind.ORB_FEATURES]  # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
  dim_augment = 7
  dim_augment_err = 6

  @staticmethod
  def generate_code(generated_dir, N=4):
    dim_augment = LocKalman.dim_augment
    dim_augment_err = LocKalman.dim_augment_err

    dim_main = LocKalman.x_initial.shape[0]
    dim_main_err = LocKalman.P_initial.shape[0]
    dim_state = dim_main + dim_augment * N
    dim_state_err = dim_main_err + dim_augment_err * N
    maha_test_kinds = LocKalman.maha_test_kinds

    name = f"{LocKalman.name}_{N}"

    # make functions and jacobians with sympy
    # state variables
    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
    cb = state[States.CLOCK_BIAS, :]
    cd = state[States.CLOCK_DRIFT, :]
    roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
    odo_scale = state[States.ODO_SCALE, :]
    acceleration = state[States.ACCELERATION, :]
    focal_scale = state[States.FOCAL_SCALE, :]
    imu_angles = state[States.IMU_OFFSET, :]
    imu_angles[0, 0] = 0
    imu_angles[2, 0] = 0
    glonass_bias = state[States.GLONASS_BIAS, :]
    glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :]
    ca = state[States.CLOCK_ACCELERATION, :]
    accel_bias = state[States.ACCELEROMETER_BIAS, :]
{'fraction_non_alphanumeric': 0.12347322267460069, 'fraction_numerical': 0.06756968368305669, 'mean_word_length': 2.300516795865633, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5814472', 'n_tokens_mistral': 5560, 'n_tokens_neox': 4908, 'n_words': 1032}