Program Listing for File StaticAlignment.hpp

↰ Return to documentation for file (src/navtk/inertial/StaticAlignment.hpp)

#pragma once

#include <navtk/inertial/AlignBase.hpp>

namespace navtk {
namespace inertial {

class StaticAlignment : public AlignBase {

public:
    StaticAlignment(const filtering::ImuModel& model = filtering::stim300_model(),
                    const double align_time          = 120.0,
                    const Matrix3& vel_cov = Matrix3{{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}});

    AlignmentStatus process(std::shared_ptr<aspn_xtensor::AspnBase> message) override;

    std::pair<bool, Matrix> get_computed_covariance(
        const CovarianceFormat format = CovarianceFormat::PINSON15NEDBLOCK) const override;

    MotionNeeded motion_needed() const override;

protected:
    virtual void calc_alignment(const aspn_xtensor::TypeTimestamp& imu_time);

    std::pair<Vector3, Matrix3> pos_stats() const;

    std::pair<Vector3, Vector3> calc_avg() const;

    double calc_average_delta_time() const;

    double align_time;

private:
    /* We've yet to get a good alignment, so keep trying to align using the latest message */
    void aligning(const aspn_xtensor::MeasurementImu& message);

    /* Check if enough IMU/GPS data has been buffered to do a static align */
    bool sufficient_data();

    /* Storage for tilt cov from sensor messages for use in solution cov output */
    Matrix3 tilt_cov;

    /* Storage for tilt cov from sensor messages for use in solution cov output */
    Matrix3 vel_cov;
};

}  // namespace inertial
}  // namespace navtk