imu_measurement_model_gravity.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2019, the mcl_3dl authors
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the copyright holder nor the names of its 
00014  *       contributors may be used to endorse or promote products derived from 
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #ifndef MCL_3DL_IMU_MEASUREMENT_MODELS_IMU_MEASUREMENT_MODEL_GRAVITY_H
00031 #define MCL_3DL_IMU_MEASUREMENT_MODELS_IMU_MEASUREMENT_MODEL_GRAVITY_H
00032 
00033 #include <memory>
00034 
00035 #include <mcl_3dl/imu_measurement_model_base.h>
00036 #include <mcl_3dl/nd.h>
00037 
00038 namespace mcl_3dl
00039 {
00040 class ImuMeasurementModelGravity : public ImuMeasurementModelBase
00041 {
00042 public:
00043   explicit ImuMeasurementModelGravity(const float acc_var)
00044     : nd_(acc_var)
00045   {
00046   }
00047 
00048   inline void setAccMeasure(const Vec3& acc_measure) final
00049   {
00050     acc_measure_ = acc_measure;
00051     acc_measure_norm_ = acc_measure.norm();
00052   }
00053 
00054   inline float measure(const State6DOF& s) const final
00055   {
00056     const Vec3 acc_estim = s.rot_.inv() * Vec3(0.0, 0.0, 1.0);
00057     const float diff = acosf(
00058         acc_estim.dot(acc_measure_) / (acc_measure_norm_ * acc_estim.norm()));
00059     return nd_(diff);
00060   }
00061 
00062 private:
00063   NormalLikelihood<float> nd_;
00064   Vec3 acc_measure_;
00065   float acc_measure_norm_;
00066 };
00067 
00068 }  // namespace mcl_3dl
00069 
00070 #endif  // MCL_3DL_IMU_MEASUREMENT_MODELS_IMU_MEASUREMENT_MODEL_GRAVITY_H


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Thu Jun 20 2019 20:04:43