/home/travis/build/lis-epfl/MAVRIC_Library/sensing/ins_kf.hpp

/*******************************************************************************
 * Copyright (c) 2009-2016, MAV'RIC Development Team
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file ins_kf.hpp
 *
 * \author MAV'RIC Team
 * \author Julien Lecoeur
 * \author Simon Pyroth
 *
 * \brief   Kalman filter for position estimation
 *
 ******************************************************************************/


#ifndef INS_KF_HPP_
#define INS_KF_HPP_


#include "status/state.hpp"
#include "drivers/gps.hpp"
#include "drivers/gps_mocap.hpp"
#include "drivers/barometer.hpp"
#include "drivers/sonar.hpp"
#include "drivers/px4flow.hpp"
#include "sensing/ahrs.hpp"
#include "sensing/ins.hpp"

#include "util/kalman.hpp"
#include "util/constants.hpp"

extern "C"
{
#include "sensing/altitude.h"
}



class INS_kf: public Kalman<11,3,3>, public INS
{
    friend void ins_telemetry_send(const INS_kf* ins, const Mavlink_stream* mavlink_stream, mavlink_message_t* msg);
public:
    struct conf_t
    {
        // Process covariance
        float sigma_bias_acc;
        float sigma_z_gnd;
        float sigma_bias_baro;
        float sigma_acc;

        // Measurement covariance
        float sigma_gps_xy;
        float sigma_gps_z;
        float sigma_gps_velxy;
        float sigma_gps_velz;
        float sigma_gps_mocap;
        float sigma_baro;
        float sigma_flow;
        float sigma_sonar;

        // Position of the origin
        global_position_t origin;
    };


    INS_kf( State& state,
            const Gps& gps,
            const Gps_mocap& gps_mocap,
            const Barometer& barometer,
            const Sonar& sonar,
            const PX4Flow& flow,
            const AHRS& ahrs,
            const conf_t config = default_config() );


    bool update(void);


    float last_update_s(void) const;


    std::array<float,3> position_lf(void) const;


    std::array<float,3> velocity_lf(void) const;


    float absolute_altitude(void) const;


    bool is_healthy(INS::healthy_t type) const;


    void init(void);


    static inline INS_kf::conf_t default_config(void);


    conf_t config_;                     
    uint32_t init_flag;                 


private:
    State&              state_;             
    const Gps&          gps_;               
    const Gps_mocap&    gps_mocap_;         
    const Barometer&    barometer_;         
    const Sonar&        sonar_;             
    const PX4Flow&      flow_;              
    const AHRS&         ahrs_;              

    std::array<float,3> pos_;
    std::array<float,3> vel_;
    float absolute_altitude_;

    Mat<3,11> H_gpsvel_;
    Mat<3,3> R_gpsvel_;
    Mat<3,3> R_mocap_;
    Mat<1,11> H_baro_;
    Mat<1,1> R_baro_;
    Mat<1,11> H_sonar_;
    Mat<1,1> R_sonar_;
    Mat<3,11> H_flow_;
    Mat<3,3> R_flow_;

    float last_accel_update_s_;             
    float last_sonar_update_s_;             
    float last_flow_update_s_;              
    float last_baro_update_s_;              
    float last_gps_pos_update_s_;           
    float last_gps_vel_update_s_;           
    float last_gps_mocap_update_s_;         
    bool  first_fix_received_;              


    float dt_;                              
    float last_update_;                     


    void predict_kf(void);


    void update_gps_pos(void);


    void update_gps_vel(void);


    void update_gps_mocap(void);


    void update_barometer(void);


    void update_sonar(void);


    void update_flow(void);
};


INS_kf::conf_t INS_kf::default_config(void)
{
    INS_kf::conf_t conf = {};

    // Process covariance (noise from state and input)
    conf.sigma_z_gnd        = 0.0f;        // previously 0.008f
    conf.sigma_bias_acc     = 0.00001f;
    conf.sigma_bias_baro    = 0.01f;       // previously 0.01f;
    conf.sigma_acc          = 5.0f;       // Measured: 0.6f (at rest 0.032f)

    // Measurement covariance   (noise from measurement)
    conf.sigma_gps_xy       = 0.04f;       // Measured: 0.316f
    conf.sigma_gps_z        = 0.13f;       // Measured: 0.879f
    conf.sigma_gps_velxy    = 0.02f;       // Measured: 0.064f
    conf.sigma_gps_velz     = 0.02f;       // Measured: 0.342f
    conf.sigma_gps_mocap    = 0.00001f;
    conf.sigma_baro         = 0.50f;       // Measured: 0.310f
    conf.sigma_flow         = 0.002f;
    conf.sigma_sonar        = 0.002f;       // Measured: 0.002f

    //default origin location (EFPL Esplanade)
    conf.origin = ORIGIN_EPFL;

    return conf;
};



static inline bool task_ins_kf_update(INS_kf* ins_kf)
{
    return ins_kf->update();
}

#endif /* INS_KF_HPP_ */