.. _program_listing_file_src_navtk_inertial_CoarseDynamicAlignment.hpp: Program Listing for File CoarseDynamicAlignment.hpp =================================================== |exhale_lsh| :ref:`Return to documentation for file ` (``src/navtk/inertial/CoarseDynamicAlignment.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include namespace navtk { namespace inertial { class CoarseDynamicAlignment : public AlignBase { public: CoarseDynamicAlignment( const filtering::ImuModel& model = filtering::stim300_model(), double static_time = 30.0, double reset_time = 300.0, DcmIntegrationMethods dcm_integration_method = DcmIntegrationMethods::FIRST_ORDER); AlignmentStatus process(std::shared_ptr message) override; std::pair get_computed_covariance( const CovarianceFormat format = CovarianceFormat::PINSON15NEDBLOCK) const override; std::pair get_imu_errors() const override; MotionNeeded motion_needed() const override; private: /* Tracks where in the dynamic alignment process we are. */ enum class Stage { INITIAL_STATIC, WAHBA_SOLVE, SOLUTION_COMPARE }; /* * Search align_buffer and pull out any data occuring after t and return in a separate vector * for use in next iteration. * * @param t Time to begin separating IMU data. * * @return Imu data with time_validity > t. */ std::vector separate_imu_after_time( const aspn_xtensor::TypeTimestamp& t); /* * Set members using first position measurement. * * @param pos Initial position measurement. */ void initialize_with_position( const not_null> pos); /* * Update move status based on current IMU data. * * @param imu Current IMU measurement. */ void update_move_status(const not_null> imu); /* * Update IMU time tracking based on current IMU data. * * @param imu Current IMU measurement. */ void update_imu_times(const not_null> imu); /* * Warn if already aligned or data too old; otherwise mechanize all prospects. If data valid * will push to align_buffer. * * @param imu Current IMU measurement. */ void mechanize_or_warn(const not_null> imu); /* * Determine calibration status and notify user with log messages. */ void update_calibration_notifications(); /* * Add new data to the alignment algorithm and estimate a new solution. * * @param pos Position measurement to attempt alignment with. * @param trim Ordered IMU data that has already been buffered by the algorithm but occurred * after the given position. */ void add_new_meas(const aspn_xtensor::MeasurementPosition& pos, const std::vector& trim); /* * Propagates a DCM using a direct integration of delta rotation values. * Uses integration method chosen at initialization. * * @param C_s_to_n Sensor to nav DCM at time k. * * @return \f$ C^n_{s_{k + 1}} \f$ */ Matrix3 integrate_dth(Matrix3 C_s_to_n); /* * Compares the error between filter position estimates and pos, which should be the last * position used to update the filters. */ void check_prospects(const aspn_xtensor::MeasurementPosition& pos); /* * Reset some variables, depending on what stage of alignment we are in, to reduce accumulated * errors and/or attempt to get out of bad state (lots of bad measurements into Wahba solution * inputs, for instance) */ void reset(const aspn_xtensor::TypeTimestamp& time); /* Initialize a temporary inertial with provided PVA and mechanize data include in trim */ filtering::NavSolution mech_align(const aspn_xtensor::MeasurementPosition& pos, const Vector3& vel, const Matrix3& C_s_to_n, const std::vector& trim); /* * Calculate the tilt covariance of the last n solutions to Wahba's problem, used to determine * solution convergence (or just lack of new information from measurements, unfortunately). */ std::pair update_last_n(const Matrix3& c_b0_to_n); /* Use the last 2 positions received to estimate the avg velocity covariance */ Matrix update_vel_cov() const; /* Estimated delta time of IMU date */ double approx_dt(); /* Initialize a new BasicInsAndFilter using provided stats and add to prospects */ void add_prospect(const filtering::NavSolution& ns, const Matrix& init_cov, const Matrix3& initial_cns); /* * Assign sol/covariance once a good prospect has been anointed. Calling this means alignment * is finished. * * @param prospect_index Index into #prospects that has the chosen solution. */ void select_prospect(const Size prospect_index); /* * Generates most recent NavSolutions using the given initial conditions. Initializes an * inertial and then mechanizes through all of align_buffer and then all of trim (in other words * all available IMU data). * * @param pos Position at the start of data in align_buffer. * @param vel Velocity at the start of data in align_buffer. * @param C_k_to_start DCM that rotates from the IMU frame at time k (start of data in * align_buffer) to initial IMU frame. * @param C_start_to_n_vec Vector of estimated DCMs that rotate from initial IMU frame to NED * frame. * @param trim Imu data occurring after that in align_buffer. * * @return A vector of NavSolutions at time of last element in /p trim, same size as /p * C_start_to_n_vec. */ std::vector generate_prospective_solutions( const aspn_xtensor::MeasurementPosition& pos, const Vector3& vel, const Matrix3& C_k_to_start, const std::vector& C_start_to_n_vec, const std::vector& trim); /* * Calculate the initial covariance to set Pinson parameters for prospective alignment * solutions. * * @param cnps Estimated initial IMU to NED frame rotations. * @param sol Solutions that covariance will be attached to. These should correspond one-to-one * with the elements of cnps. * @param test_cov Initial tilt covariance estimate. Assumed to be valid for all solutions. * * @return A collection of 15x15 Pinson PVA matrices corresponding to each element of sol. */ std::vector calc_initial_solution_cov(const std::vector& cnps, const std::vector& sol, const Matrix3& tilt_cov); /* * Use measured (IMU) and reference (pos derived) force measurements to update the * Wahba/Davenport function inputs and estimate the initial IMU to nav DCM. */ void update_wahba_inputs(); // Number of position measurements received after movement detected Size call_num = 0; // DCM from IMU frame at t(k) to initial IMU frame, calculated from integrating dth measurements Matrix3 C_k_to_start = eye(3); // DCM from IMU frame at t(k - 1) to initial IMU frame, calculated from integrating dth // measurements Matrix3 C_km1_to_start = eye(3); // Sum of outer products of time-synced specific forces in IMU and NED frames; input to Wahba // solver (davenport_q method) Matrix3 B = zeros(3, 3); // Sum of cross terms of time-synced specific forces in IMU and NED frames; i.e. same vectors // used to form outer products that form B; used in davenport solver but not original Wahba // solver Vector3 cross_terms = zeros(3); // Vector of estimated initial IMU orientations from Wahba solver std::vector est_cnps; // How long to wait, in seconds, before calling reset(). Set by user at initialization. double give_up_time; // Counter for tracking stored solutions, this time of last_n_solutions Size sol_ind = 0; // vector of last n (n = num_solutions_in_cov) IMU to nav estimated DCMs std::vector last_n_solutions; // vector of full INS + filters, initialized using position measurements and estimated DCMs from // Wahba solutions and mechanized dth data the best of these is the source of the computed // alignment std::vector prospects; // Covariance from the best of prospects, returned by get_computed_covariance Matrix full_cov = zeros(15, 15); // Average delta_v from stationary data collected by movement_detector, used to estimate initial // biases Vector3 stat_dv_mean = zeros(3); // Average delta_theta from stationary data collected by movement_detector, used to estimate // initial biases Vector3 stat_dth_mean = zeros(3); // IMU errors estimated by the best of prospects; empty until one is selected. ImuErrors imu_bs; // Time of first IMU data received aspn_xtensor::TypeTimestamp first_imu_time = aspn_xtensor::to_type_timestamp(); // Time of latest IMU data received aspn_xtensor::TypeTimestamp latest_imu_time = aspn_xtensor::to_type_timestamp(); // Number of IMU data received; used with imu_time data to estimate IMU dt and to trigger // messages to console Size num_imu_received = 0; // Max value of size_t, to make sure we don't roll over our IMU counter Size max_imu_count = 0; // Detects movement based on sensor data MovementDetectorImu detector; // Immediate last status of detection algorithm MovementStatus last_status = MovementStatus::NOT_MOVING; // Currently set to true when moving_ctr > 100, kicks off main algorithm bool movement_detected = false; /* Flag for tracking if user has been notified that stationary period has completed */ bool sustained_notified = false; /* Ctor supplied static alignment time */ double static_time; /* How often user is updated on calibration status (s), based on IMU message time. */ double calib_notify_period = 5.0; /* Last time we notified of stationary calibration status */ aspn_xtensor::TypeTimestamp calib_last_notify = aspn_xtensor::to_type_timestamp(); /* Magic Numbers */ // Assumed value of dt if not enough IMU data to actually calculate it. Updated over time. double default_dt = 0.01; // Threshold for the minimum down tilt sigma needed to declare the Wahba solution 'stable' and // to enter the next phase (filtering and testing residuals) (deg) double down_tilt_sig_thresh = 5.0; // Number of measurements that need to be received before moving on to filtering of possible // solutions (Trying to assure variation in Wahba inputs) Size min_meas_before_solution_gen = 50; // When comparing solutions, at least one error norm must be outside the max (by axis) sigma // times this number before further testing is done (in other words making sure at least one // solution looks sufficiently bad that another one can be called 'good' by comparison) double position_sigma_multiplier = 2.0; // Ratio that a 'good' solution must beat all other 'bad' solutions by to be declared the best // estimate (in other words, the norm position residual on all bad solutions must be error_ratio // times greater than the error norm of teh good solution) double error_ratio = 15.0; // Dictates size of last_n_solutions Size num_solutions_in_cov = 10; // Collects stationary data during bias calibration period std::vector stationary_dv; std::vector stationary_dth; // Tracks last time movement detector switched from something other than moving to moving, to // allow for monitoring for sustained movement aspn_xtensor::TypeTimestamp moving_start_time = aspn_xtensor::to_type_timestamp(); // Number of seconds of sustained movement required to move from stationary period to wahba // solution calculation double required_moving_period = 1.0; // Function that integrates delta theta measurements so they can be used to update a DCM. std::function dtheta_integrator; // Convenience class that calculates a bunch of the nav data required to generate Wahba solver // inputs std::shared_ptr iteration_data = nullptr; // What stage of dynamic alignment we are in Stage current_stage = Stage::INITIAL_STATIC; // Time since we last did a hard reset (or changed current_stage) aspn_xtensor::TypeTimestamp time_since_last_reset = aspn_xtensor::to_type_timestamp(); // Collection of time-ordered linearization points over the period that we are attempting to // calculate wahba solutions; used to propagate a covariance forward from the initial time to // the `current` so we can kick off filters. std::vector< std::pair> lin_points; }; } // namespace inertial } // namespace navtk