Program Listing for File ManualHeadingAlignment.hpp

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

#pragma once

#include <memory>

#include <navtk/filtering/containers/NavSolution.hpp>
#include <navtk/inertial/StaticAlignment.hpp>
#include <navtk/tensors.hpp>

namespace navtk {
namespace inertial {

class ManualHeadingAlignment : public StaticAlignment {
public:
    ManualHeadingAlignment(const double heading,
                           const double heading_sigma       = 0.017453292519943295,
                           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}});

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

    std::pair<bool, ImuErrors> get_imu_errors() const override;

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

private:
    /* Storage for user-provided heading */
    double heading;

    /* Storage for user-provided heading sigma */
    double heading_sigma;

    /* Sets `computed_alignment` and `alignment_status` if requirements met */
    void calc_align();
};
}  // namespace inertial
}  // namespace navtk