<jupyter_start><jupyter_text>********************************************************************************IMU-GNSS Sensor-Fusion on the KITTI Dataset********************************************************************************Goals of this script:- apply the UKF for estimating the 3D pose, velocity and sensor biases of a  vehicle on real data.- efficiently propagate the filter when one part of the Jacobian is already  known. - efficiently update the system for GNSS position.*We assume the reader is already familiar with the approach described in thetutorial and in the 2D SLAM example.*This script proposes an UKF to estimate the 3D attitude, the velocity, and theposition of a rigid body in space from inertial sensors and positionmeasurement.We use the KITTI data that can be found in the `iSAM repo`_(examples folder). Import==============================================================================<jupyter_code>from scipy.linalg import block_diag
import ukfm
import numpy as np
import matplotlib
ukfm.set_matplotlib_config()<jupyter_output><empty_output><jupyter_text>Model and Data==============================================================================This script uses the :meth:`~ukfm.IMUGNSS` model that loads the KITTI datafrom text files. The model is the standard 3D kinematics model based oninertial inputs.<jupyter_code>MODEL = ukfm.IMUGNSS
# observation frequency (Hz)
GNSS_freq = 1
# load data
omegas, ys, one_hot_ys, t = MODEL.load(GNSS_freq)
N = t.shape[0]
# IMU noise standard deviation (noise is isotropic)
imu_std = np.array([0.01,     # gyro (rad/s)
                    0.05,     # accelerometer (m/s^2)
                    0.000001, # gyro bias (rad/s^2)
                    0.0001])  # accelerometer bias (m/s^3)
# GNSS noise standard deviation (m)
GNSS_std = 0.05<jupyter_output><empty_output><jupyter_text>The state and the input contain the following variables:.. highlight:: python.. code-block:: python   states[n].Rot      3d orientation (matrix)   states[n].v        3d velocity   states[n].p        3d position   states[n].b_gyro   gyro bias   states[n].b_acc    accelerometer bias   omegas[n].gyro     vehicle angular velocities   omegas[n].acc      vehicle specific forcesA measurement ``ys[k]`` contains a GNSS (position) measurement. Filter Design and Initialization------------------------------------------------------------------------------We now design the UKF on parallelizable manifolds. This script embeds thestate in $SO(3) \times \mathbb{R}^{12}$, such that:* the retraction $\varphi(.,.)$ is the $SO(3)$ exponential for  orientation, and the vector addition for the remaining part of the  state.* the inverse retraction $\varphi^{-1}_.(.)$ is the $SO(3)$  logarithm for orientation and the vector subtraction for the remaining part  of the state.Remaining parameter setting is standard.<jupyter_code># propagation noise covariance matrix
Q = block_diag(imu_std[0]**2*np.eye(3), imu_std[1]**2*np.eye(3),
               imu_std[2]**2*np.eye(3), imu_std[3]**2*np.eye(3))
# measurement noise covariance matrix
R = GNSS_std**2 * np.eye(3)<jupyter_output><empty_output><jupyter_text>We use the UKF that is able to infer Jacobian to speed up the update step, seethe 2D SLAM example.<jupyter_code># sigma point parameters
alpha = np.array([1e-3, 1e-3, 1e-3, 1e-3, 1e-3])
# for propagation we need the all state
red_idxs = np.arange(15)  # indices corresponding to the full state in P
# for update we need only the state corresponding to the position
up_idxs = np.array([6, 7, 8])<jupyter_output><empty_output><jupyter_text>We initialize the state with zeros biases. The initial covariance is non-nullas the state is not perfectly known.<jupyter_code># initial uncertainty matrix
P0 = block_diag(0.01*np.eye(3), 1*np.eye(3), 1*np.eye(3),
                0.001*np.eye(3), 0.001*np.eye(3))
# initial state
state0 = MODEL.STATE(
    Rot=np.eye(3),
    v=np.zeros(3),
    p=np.zeros(3),
    b_gyro=np.zeros(3),
    b_acc=np.zeros(3))<jupyter_output><empty_output><jupyter_text>As the noise affecting the dynamic of the biases is trivial (it is theidentity), we set our UKF with a reduced propagation noise covariance, andmanually set the remaining part of the Jacobian.<jupyter_code># create the UKF
ukf = ukfm.JUKF(state0=state0, P0=P0, f=MODEL.f, h=MODEL.h, Q=Q[:6, :6],
                phi=MODEL.phi, alpha=alpha, red_phi=MODEL.phi,
                red_phi_inv=MODEL.phi_inv, red_idxs=red_idxs,
                up_phi=MODEL.up_phi, up_idxs=up_idxs)
# set variables for recording estimates along the full trajectory
ukf_states = [state0]
ukf_Ps = np.zeros((N, 15, 15))
ukf_Ps[0] = P0
# the part of the Jacobian that is already known.
G_const = np.zeros((15, 6))
G_const[9:] = np.eye(6)<jupyter_output><empty_output><jupyter_text>Filtering==============================================================================The UKF proceeds as a standard Kalman filter with a for loop.<jupyter_code># measurement iteration number
k = 1
for n in range(1, N):
    # propagation
    dt = t[n]-t[n-1]
    ukf.state_propagation(omegas[n-1], dt)
    ukf.F_num(omegas[n-1], dt)
    # we assert the reduced noise covariance for computing Jacobian.
    ukf.Q = Q[:6, :6]
    ukf.G_num(omegas[n-1], dt)
    # concatenate Jacobian
    ukf.G = np.hstack((ukf.G, G_const*dt))
    # we assert the full noise covariance for uncertainty propagation.
    ukf.Q = Q
    ukf.cov_propagation()
    # update only if a measurement is received
    if one_hot_ys[n] == 1:
        ukf.update(ys[k], R)
        k = k + 1
    # save estimates
    ukf_states.append(ukf.state)
    ukf_Ps[n] = ukf.P<jupyter_output><empty_output><jupyter_text>Results------------------------------------------------------------------------------We plot the estimated trajectory.<jupyter_code>MODEL.plot_results(ukf_states, ys)<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1885665529010239, 'fraction_numerical': 0.020819112627986348, 'mean_word_length': 3.8800999167360533, 'pattern_counts': {'":': 0, '<': 33, '<?xml version=': 0, '>': 33, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '8', 'dir': 'jupyter-structured-clean-dedup', 'id': '581182', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 1936, 'n_tokens_neox': 1796, 'n_words': 567, 'path': 'docs/_downloads/d3740909c02838cd0c146ae6634ed841/imugnss.ipynb', 'repo_name': 'CAOR-MINES-ParisTech/ukfm', 'size': '5860'}