<commit_before>/*
 * Copyright (c) 2014 Zubax, zubax.com
 * Please refer to the file LICENSE for terms and conditions.
 * Author: <NAME> <<EMAIL>>
 */

#pragma once

#include <ros/ros.h>
#include <ros/console.h>
#include <eigen3/Eigen/Eigen>
#include <cmath>
#include "mathematica.hpp"
#include "debug_publisher.hpp"
#include "linear_algebra.hpp"
#include "exception.hpp"
#include "state_vector_autogenerated.hpp"

namespace zubax_posekf
{
/**
 * Filter output - twist; compatible with geometry_msgs/TwistWithCovariance.
 * - Velocity of IMU in the world frame, transformed into the IMU frame
 * - Angular velocity of IMU in the IMU frame
 * - Covariance: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
 */
struct Twist
{
    Vector3 linear;
    Vector3 angular;
    Matrix<6, 6> linear_angular_cov;

    Twist()
    {
        linear.setZero();
        angular.setZero();
        linear_angular_cov.setZero();
    }
};

/**
 * Filter output - pose; compatible with geometry_msgs/PoseWithCovariance.
 * - Position of IMU in the world frame
 * - Orientation of IMU in the world frame
 * - Covariance: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
 */
struct Pose
{
    Vector3 position;
    Quaternion orientation;
    Matrix<6, 6> position_orientation_cov;

    Pose()
    {
        position.setZero();
        orientation.setIdentity();
        position_orientation_cov.setZero();
    }
};

/**
 * The business end.
 */
class Filter
{
    DebugPublisher debug_pub_;

    StateVector state_;
    Matrix<StateVector::Size, StateVector::Size> P_;
    Matrix<StateVector::Size, StateVector::Size> Q_;

    const Scalar MaxGyroDrift = 0.1;
    const Scalar MaxAccelDrift = 2.0;
    const Scalar MaxCovariance = 1e9;
    const Scalar MinVariance = 1e-9;

    Scalar state_timestamp_ = 0.0;
    bool initialized_ = false;

    static Vector3 constrainDrift(Vector3 vec, const Scalar limit, const char* name)
    {
        for (int i = 0; i < 3; i++)
        {
            if (!checkRangeAndConstrainSymmetric(vec[i], limit))
            {
                ROS_WARN_THROTTLE(1, "Drift is too high [%s]", name);
            }
        }
        return vec;
    }

    void normalizeAndCheck()
    {
        state_.normalize();

        state_.bw(constrainDrift(state_.bw(), MaxGyroDrift, "gyro"));

        state_.ba(constrainDrift(state_.ba(), MaxAccelDrift, "accel"));

        // P validation
        if (!validateAndFixCovarianceMatrix(P_, MaxCovariance, MinVariance))
        {
            ROS_ERROR_STREAM_THROTTLE(1, "Matrix P has been fixed\n" << P_.format(Eigen::IOFormat(2)));
        }

        debug_pub_.publish("x", state_.x);
        debug_pub_.publish("P", P_);
        debug_pub_.publish("Q", Q_);

        enforce("Non-finite states",
                std::isfinite(state_.x.sum()) &&
                std::isfinite(P_.sum()) &&
                std::isfinite(Q_.sum()));

        enforce("Invalid timestamp", std::isfinite(state_timestamp_) && (state_timestamp_ > 0.0));
    }

    template <int NumDims>
    void performMeasurementUpdate(const Vector<NumDims>& y,
                                  const Matrix<NumDims, NumDims>& R,
                                  const Matrix<NumDims, StateVector::Size>& H,
                                  const char* const source_name)
    {
        ROS_ASSERT(initialized_);

        /*
         * Ensure that R is symmetric, then validate
         */
        const Matrix<NumDims, NumDims> R_sym = 0.5 * (R + R.transpose());
        {
            bool R_sym_ok = true;
            for (int i = 0; i < NumDims; i++)
            {
                if (R_sym(i, i) <= 0)
                {
                    R_sym_ok = false;
                    break;
                }
            }

            if (!R_sym_ok || !std::isfinite(R_sym.sum()))
            {
                std::ostringstream os;
                os << "Measurement R_sym [" << source_name << "]:\n" << R_sym;
                throw Exception(os.str());
            }
        }

        /*
         * Compute S inverse with validation
         */
        Matrix<NumDims, NumDims> S_inv;
        {
            const Matrix<NumDims, NumDims> S = H * P_ * H.transpose() + R_sym;
            bool s_is_invertible = false;
            S.computeInverseWithCheck(S_inv, s_is_invertible);
            if (!s_is_invertible)
            {
                std::ostringstream os;
                os << "S is not invertible (numerical failure?) [" << source_name << "]:\n" << S
                    << "\nR_sym:\n" << R_sym;
                throw Exception(os.str());
            }
        }

        /*
         * Kalman update equations
         * Joseph form is used instead of the simplified form to improve numerical stability
         */
        const auto K = static_cast<Matrix<StateVector::Size, NumDims> >(P_ * H.transpose() * S_inv);

        state_.x = state_.x + K * y;

        const Matrix<StateVector::Size, StateVector::Size> IKH = decltype(P_)::Identity() - K * H;
        P_ = IKH * P_ * IKH.transpose() + K * R_sym * K.transpose();

        normalizeAndCheck();

        std::cout << "P:\n" << P_.format(Eigen::IOFormat(2)) << "\n" << std::endl;
        std::cout << "x:\n" << state_.x.transpose().format(Eigen::IOFormat(2)) << "\n" << std::endl;
    }

public:
    Filter()
    {
        // Initial state - zero everything, null rotations
        state_.qwi(Quaternion(1, 0, 0, 0));
        state_.qvw(Quaternion(1, 0, 0, 0));

        // Initial P
        P_.setZero();
        P_ = StateVector::Pinitdiag().asDiagonal();

        // Initial Q
        // TODO: runtime Q estimation
        Q_ = StateVector::Qmindiag().asDiagonal();
    }

    bool isInitialized() const { return initialized_; }

    void initialize(Scalar timestamp, const Quaternion& orientation)
    {
        ROS_ASSERT(!initialized_);
        initialized_ = true;

        state_timestamp_ = timestamp;<commit_msg>No spanning to the terminal<commit_after>/*
 * Copyright (c) 2014 Zubax, zubax.com
 * Please refer to the file LICENSE for terms and conditions.
 * Author: <NAME> <<EMAIL>>
 */

#pragma once

#include <ros/ros.h>
#include <ros/console.h>
#include <eigen3/Eigen/Eigen>
#include <cmath>
#include "mathematica.hpp"
#include "debug_publisher.hpp"
#include "linear_algebra.hpp"
#include "exception.hpp"
#include "state_vector_autogenerated.hpp"

namespace zubax_posekf
{
/**
 * Filter output - twist; compatible with geometry_msgs/TwistWithCovariance.
 * - Velocity of IMU in the world frame, transformed into the IMU frame
 * - Angular velocity of IMU in the IMU frame
 * - Covariance: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
 */
struct Twist
{
    Vector3 linear;
    Vector3 angular;
    Matrix<6, 6> linear_angular_cov;

    Twist()
    {
        linear.setZero();
        angular.setZero();
        linear_angular_cov.setZero();
    }
};

/**
 * Filter output - pose; compatible with geometry_msgs/PoseWithCovariance.
 * - Position of IMU in the world frame
 * - Orientation of IMU in the world frame
 * - Covariance: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
 */
struct Pose
{
    Vector3 position;
    Quaternion orientation;
    Matrix<6, 6> position_orientation_cov;

    Pose()
    {
        position.setZero();
        orientation.setIdentity();
        position_orientation_cov.setZero();
    }
};

/**
 * The business end.
 */
class Filter
{
    DebugPublisher debug_pub_;

    StateVector state_;
    Matrix<StateVector::Size, StateVector::Size> P_;
    Matrix<StateVector::Size, StateVector::Size> Q_;

    const Scalar MaxGyroDrift = 0.1;
    const Scalar MaxAccelDrift = 2.0;
    const Scalar MaxCovariance = 1e9;
    const Scalar MinVariance = 1e-9;

    Scalar state_timestamp_ = 0.0;
    bool initialized_ = false;

    static Vector3 constrainDrift(Vector3 vec, const Scalar limit, const char* name)
    {
        for (int i = 0; i < 3; i++)
        {
            if (!checkRangeAndConstrainSymmetric(vec[i], limit))
            {
                ROS_WARN_THROTTLE(1, "Drift is too high [%s]", name);
            }
        }
        return vec;
    }

    void normalizeAndCheck()
    {
        state_.normalize();

        state_.bw(constrainDrift(state_.bw(), MaxGyroDrift, "gyro"));

        state_.ba(constrainDrift(state_.ba(), MaxAccelDrift, "accel"));

        // P validation
        if (!validateAndFixCovarianceMatrix(P_, MaxCovariance, MinVariance))
        {
            ROS_ERROR_STREAM_THROTTLE(1, "Matrix P has been fixed\n" << P_.format(Eigen::IOFormat(2)));
        }

        debug_pub_.publish("x", state_.x);
        debug_pub_.publish("P", P_);
        debug_pub_.publish("Q", Q_);

        enforce("Non-finite states",
                std::isfinite(state_.x.sum()) &&
                std::isfinite(P_.sum()) &&
                std::isfinite(Q_.sum()));

        enforce("Invalid timestamp", std::isfinite(state_timestamp_) && (state_timestamp_ > 0.0));
    }

    template <int NumDims>
    void performMeasurementUpdate(const Vector<NumDims>& y,
                                  const Matrix<NumDims, NumDims>& R,
                                  const Matrix<NumDims, StateVector::Size>& H,
                                  const char* const source_name)
    {
        ROS_ASSERT(initialized_);

        /*
         * Ensure that R is symmetric, then validate
         */
        const Matrix<NumDims, NumDims> R_sym = 0.5 * (R + R.transpose());
        {
            bool R_sym_ok = true;
            for (int i = 0; i < NumDims; i++)
            {
                if (R_sym(i, i) <= 0)
                {
                    R_sym_ok = false;
                    break;
                }
            }

            if (!R_sym_ok || !std::isfinite(R_sym.sum()))
            {
                std::ostringstream os;
                os << "Measurement R_sym [" << source_name << "]:\n" << R_sym;
                throw Exception(os.str());
            }
        }

        /*
         * Compute S inverse with validation
         */
        Matrix<NumDims, NumDims> S_inv;
        {
            const Matrix<NumDims, NumDims> S = H * P_ * H.transpose() + R_sym;
            bool s_is_invertible = false;
            S.computeInverseWithCheck(S_inv, s_is_invertible);
            if (!s_is_invertible)
            {
                std::ostringstream os;
                os << "S is not invertible (numerical failure?) [" << source_name << "]:\n" << S
                    << "\nR_sym:\n" << R_sym;
                throw Exception(os.str());
            }
        }

        /*
         * Kalman update equations
         * Joseph form is used instead of the simplified form to improve numerical stability
         */
        const auto K = static_cast<Matrix<StateVector::Size, NumDims> >(P_ * H.transpose() * S_inv);

        state_.x = state_.x + K * y;

        const Matrix<StateVector::Size, StateVector::Size> IKH = decltype(P_)::Identity() - K * H;
        P_ = IKH * P_ * IKH.transpose() + K * R_sym * K.transpose();

        normalizeAndCheck();

//        std::cout << "P:\n" << P_.format(Eigen::IOFormat(2)) << "\n" << std::endl;
//        std::cout << "x:\n" << state_.x.transpose().format(Eigen::IOFormat(2)) << "\n" << std::endl;
    }

public:
    Filter()
    {
        // Initial state - zero everything, null rotations
        state_.qwi(Quaternion(1, 0, 0, 0));
        state_.qvw(Quaternion(1, 0, 0, 0));

        // Initial P
        P_.setZero();
        P_ = StateVector::Pinitdiag().asDiagonal();

        // Initial Q
        // TODO: runtime Q estimation
        Q_ = StateVector::Qmindiag().asDiagonal();
    }

    bool isInitialized() const { return initialized_; }

    void initialize(Scalar timestamp, const Quaternion& orientation)
    {
        ROS_ASSERT(!initialized_);
        initialized_ = true;

        state_timestamp_ = timestamp;
{'fraction_non_alphanumeric': 0.12518828451882846, 'fraction_numerical': 0.007531380753138075, 'mean_word_length': 2.0557402198926105, 'pattern_counts': {'":': 0, '<': 127, '<?xml version=': 0, '>': 47, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '602301', 'n_tokens_mistral': 3832, 'n_tokens_neox': 3636, 'n_words': 903}