Function initialize_ukf 

pub fn initialize_ukf(
    initial_pose: TestDataRecord,
    attitude_covariance: Option<Vec<f64>>,
    imu_biases: Option<Vec<f64>>,
) -> UKF

Helper function to initialize a UKF for closed-loop mode.

This function sets up the Unscented Kalman Filter (UKF) with initial pose, attitude covariance, and IMU biases based on the provided TestDataRecord. It initializes the UKF with position, velocity, attitude, and covariance matrices. Optional parameters for attitude covariance and IMU biases can be provided to customize the filter’s initial state.

Arguments

• initial_pose - A TestDataRecord containing the initial pose information.
• attitude_covariance - Optional vector of f64 representing the initial attitude covariance (default is a small value).
• imu_biases - Optional vector of f64 representing the initial IMU biases (default is a small value).

Returns

• UKF - An instance of the Unscented Kalman Filter initialized with the provided parameters.