├── ukf
    ├── config
    │   └── ukf.yaml
    ├── launch
    │   └── ukf.launch
    ├── src
    │   ├── ukf_node.cpp
    │   └── ukf.cpp
    ├── CMakeLists.txt
    ├── package.xml
    └── include
    │   └── ukf
    │       └── ukf.h
└── README.md


/ukf/config/ukf.yaml:
--------------------------------------------------------------------------------
1 | init_state_provided: false
2 | 
3 | imu_topic: /imu/data
4 | gnss_topic: /fix
5 | odom_topic: /odometry/car
6 | 


--------------------------------------------------------------------------------
/ukf/launch/ukf.launch:
--------------------------------------------------------------------------------
 1 | <!-- -->
 2 | <launch>
 3 | 
 4 | 	<node pkg="ukf" type="ukf_node" name="ukf_node" output="screen" clear_params="true">
 5 | 		<rosparam command="load" file="$(find ukf)/config/ukf.yaml" />
 6 | 	</node>
 7 | 
 8 | 
 9 | </launch>
10 | 
11 | 


--------------------------------------------------------------------------------
/ukf/src/ukf_node.cpp:
--------------------------------------------------------------------------------
 1 | /**********************************
 2 | 
 3 |     Created on : 24th April 2020 
 4 |     Author     : Krishna Sandeep
 5 | 
 6 | **********************************/
 7 | 
 8 | #include <ros/ros.h>
 9 | 
10 | #include "ukf/ukf.h"
11 | 
12 | int main(int argc, char** argv)
13 | {
14 |     ros::init(argc, argv, "ukf_node");
15 |     ros::NodeHandle node;
16 |     ros::NodeHandle private_nh("~");
17 | 
18 |     UKF ukf(node, private_nh); //instance of UKF class
19 | 
20 |     ros::Rate loop_rate(30);
21 | 
22 |     while(ros::ok())
23 |     {
24 |         ros::spinOnce(); //invokes callback
25 |         ukf.estimateVehicleState(); //runs the UKF estimation
26 |         loop_rate.sleep();
27 |     }
28 | 
29 |     return 0;
30 | }


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # UKF
 2 | Unscented Kalman Filter using IMU and GNSS data for vehicle or mobile robot localization.
 3 | 
 4 | Requirements: Eigen3, ROS
 5 | 
 6 | Input: sensor_msgs/NavSatFix, sensor_msgs/Imu
 7 | 
 8 | Output: nav_msgs/Odometry
 9 | 
10 | Key Features:
11 | 
12 | 1. Assumes 2D motion.
13 | 2. Initializes the state{position x, position y, heading angle, velocity x, velocity y} to (0.0, 0.0, yaw, 0.0, 0.0) with the yaw from IMU at the start of the program if no initial state is provided.
14 | 3. Uses acceleration and yaw rate data from IMU in the prediction step.
15 | 4. GNSS data is used for correction.
16 | 5. Yaw from the IMU is only used to initialize and not in correction step.
17 | 
18 | How to use:
19 | 
20 |     1. Clone the repository in to your workspace
21 |     2. catkin_make #compile
22 |     3. Edit the parameters in config/ukf.yaml to your suiting. Note that not all the parameters used in the code are not present in the yaml file.
23 |     4. roslaunch ukf ukf.launch #to run the code
24 | 
25 | 


--------------------------------------------------------------------------------
/ukf/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(ukf)
 3 | 
 4 | ## Compile as C++11, supported in ROS Kinetic and newer
 5 |  add_compile_options(-std=c++11)
 6 | 
 7 | ## Find catkin macros and libraries
 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 9 | ## is used, also find other catkin packages
10 | find_package(catkin REQUIRED COMPONENTS
11 |   roscpp
12 |   std_msgs
13 |   geometry_msgs
14 |   sensor_msgs
15 |   nav_msgs
16 |   tf2
17 | )
18 | 
19 | ## System dependencies are found with CMake's conventions
20 | find_package(Eigen3 REQUIRED)
21 | 
22 | ###################################
23 | ## catkin specific configuration ##
24 | ###################################
25 | ## The catkin_package macro generates cmake config files for your package
26 | ## Declare things to be passed to dependent projects
27 | ## INCLUDE_DIRS: uncomment this if your package contains header files
28 | ## LIBRARIES: libraries you create in this project that dependent projects also need
29 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
30 | ## DEPENDS: system dependencies of this project that dependent projects also need
31 | catkin_package(
32 |   INCLUDE_DIRS include
33 | #  LIBRARIES ukf
34 |   CATKIN_DEPENDS roscpp std_msgs
35 |   DEPENDS system_lib
36 | )
37 | 
38 | ###########
39 | ## Build ##
40 | ###########
41 | 
42 | ## Specify additional locations of header files
43 | ## Your package locations should be listed before other locations
44 | include_directories(
45 |   include
46 |   ${catkin_INCLUDE_DIRS}
47 |   ${EIGEN3_INCLUDE_DIRS}
48 | )
49 | 
50 | add_executable(ukf_node src/ukf_node.cpp src/ukf.cpp)
51 | target_link_libraries(ukf_node ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES})
52 | add_dependencies(ukf_node ${catkin_EXPORTED_TARGETS})
53 | 
54 | 


--------------------------------------------------------------------------------
/ukf/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>ukf</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The ukf package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="user@todo.todo">user</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/ukf</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>roscpp</build_depend>
53 |   <build_depend>std_msgs</build_depend>
54 |   <build_export_depend>roscpp</build_export_depend>
55 |   <build_export_depend>std_msgs</build_export_depend>
56 |   <exec_depend>roscpp</exec_depend>
57 |   <exec_depend>std_msgs</exec_depend>
58 | 
59 | 
60 |   <!-- The export tag contains other, unspecified, tags -->
61 |   <export>
62 |     <!-- Other tools can request additional information be placed here -->
63 | 
64 |   </export>
65 | </package>
66 | 


--------------------------------------------------------------------------------
/ukf/include/ukf/ukf.h:
--------------------------------------------------------------------------------
 1 | /**********************************
 2 | 
 3 |     Created on : 24th April 2020 
 4 |     Author     : Krishna Sandeep
 5 | 
 6 | **********************************/
 7 | 
 8 | #ifndef UKF_UKF_H
 9 | #define UKF_UKF_H
10 | 
11 | #include <ros/ros.h>
12 | #include <sensor_msgs/Imu.h>
13 | #include <sensor_msgs/NavSatFix.h>
14 | #include <nav_msgs/Odometry.h>
15 | 
16 | #include <string>
17 | #include <utility>
18 | 
19 | #include <Eigen/Dense>
20 | 
21 | using namespace std; 
22 | using namespace Eigen;
23 | 
24 | /*
25 | --------- Assuming 2D motion, Vehicle state {x,y,theta,vx,vy} i.e., {position x, position y, heading angle, velocity x, velocity y}
26 | */
27 | 
28 | typedef Matrix<double, 5, 5> Matrix5d;
29 | typedef Matrix<double, 5, 1> Vector5d;
30 | 
31 | /* state consists of 5 elements [x, y, theta, vx, vy] in order and a 5x5 covariance matrix
32 |    (x,y)   : position in map frame
33 |    theta   : heading angle (ENU convention)
34 |    (vx,vy) : velocity in base_link frame
35 | */
36 | class VehicleState
37 | {
38 |     public:
39 |         VehicleState();
40 |         ~VehicleState(){}
41 | 
42 |         Vector5d vec;
43 |         Matrix5d cov_mat; //covariance matrix
44 | };
45 | 
46 | class MMInput
47 | {
48 |     public:
49 |         MMInput();
50 |         ~MMInput(){}
51 | 
52 |         double ax; //acceleration
53 |         double ay;
54 |         double omega; //yaw rate
55 |         double delT; //time interval
56 | };
57 | 
58 | class UKF
59 | {
60 |     public:
61 |         UKF(ros::NodeHandle node, ros::NodeHandle private_nh);
62 |         ~UKF(){}
63 | 
64 |         void estimateVehicleState(); //runs the UKF estimation
65 |     
66 |     private:
67 |         void imuCallback(const sensor_msgs::Imu::ConstPtr& msg); //IMU callback
68 |         void gnssCallback(const sensor_msgs::NavSatFix::ConstPtr& fix); //GNSS callback
69 | 
70 |         void initVehicleState(const double yaw); //Initializes the Vehicle state to default values
71 |         void predict(); //the prediction step
72 |         Vector5d applyMotionModel(Vector5d vec); //applies the model model
73 |         void correct(const double x, const double y); //the correction step
74 |         void publishVehicleState(); //publishes the vehicle state as a nav_msgs::Odometry message
75 | 
76 |         ros::Subscriber gnss_sub; //GNSS subscriber
77 |         ros::Subscriber imu_sub; //IMU data subscriber
78 |         ros::Publisher odom_pub; //publishes nav_msgs::Odometry msgs
79 |         string _imu_topic, _gnss_topic, _odom_topic; //topic names to subscribe/publish
80 |         double _noise_var_pos, _noise_var_yaw, _noise_var_vel, _noise_var_meas; //noise variances for position, yaw, velocity and measurement
81 |         bool _init_state_provided; //True if initial state is provided
82 |         double _init_x, _init_y, _init_yaw, _init_velx, _init_vely; //initial values for state
83 |         double _init_var_pos, _init_var_yaw, _init_var_vel; //initial variances for position, yaw, velocity
84 | 
85 |         VehicleState _curr_state, _predicted_state;
86 |         pair<double, double> _init_utm, _curr_utm; //Initial and current UTM reading in (easting, northing)
87 |         pair<double, double> _vp_at_first_gnss; //vehicle position when the 1st GNSS reading is recieved
88 |         bool _is_first_imu, _is_first_gnss; //boolean to verify if this is the first IMU, GNSS reading respectively
89 |         bool _imu_available, _gnss_available; //true if a new imu/gnss reading is available
90 |         double _prev_imu_time, _curr_imu_time; //Timestamps of two consecutive IMU readings
91 |         MMInput _mmi; //current motion model input
92 |         Matrix<double, 5, 11> _propagated_sigma_pt_mat; //matrix to store the sigma points propagated using the motion model
93 |         Matrix5d _process_noise_mat; //Covariance matrix for the process noise
94 |         Matrix2d _meas_noise_mat; //Covariance matrix for the measurement noise
95 | };
96 | 
97 | #endif


--------------------------------------------------------------------------------
/ukf/src/ukf.cpp:
--------------------------------------------------------------------------------
  1 | /**********************************
  2 | 
  3 |     Created on : 24th April 2020 
  4 |     Author     : Krishna Sandeep
  5 | 
  6 | **********************************/
  7 | 
  8 | #include "ukf/ukf.h"
  9 | 
 10 | #include <tf2/LinearMath/Quaternion.h>
 11 | #include <tf2/LinearMath/Matrix3x3.h>
 12 | #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 13 | 
 14 | #include <gps_common/conversions.h>
 15 | 
 16 | #include <cmath>
 17 | 
 18 | #include <Eigen/LU>
 19 | 
 20 | /* @brief Converts quaternion to euler */
 21 | double getYaw(geometry_msgs::Quaternion q_msg)
 22 | {
 23 |    tf2::Quaternion quat;
 24 |    tf2::fromMsg(q_msg, quat);
 25 | 
 26 |    tf2::Matrix3x3 mat(quat);
 27 |    double roll, pitch, yaw;
 28 |    mat.getRPY(roll, pitch, yaw);
 29 | 
 30 |    return yaw;
 31 | }
 32 | 
 33 | /* @brief Constructor */
 34 | VehicleState::VehicleState()
 35 | {
 36 |     vec = Vector5d::Zero();
 37 |     cov_mat = 1e-4*Matrix5d::Identity(); //small variances
 38 | }
 39 | 
 40 | /* @brief Constructor */
 41 | MMInput::MMInput()
 42 | {
 43 |     ax = 0.0;
 44 |     ay = 0.0;
 45 |     omega = 0.0;
 46 |     delT = 0.0;
 47 | }
 48 | 
 49 | /* @brief Constructor */
 50 | UKF::UKF(ros::NodeHandle node, ros::NodeHandle private_nh)
 51 | {
 52 |     /* Loading parameters */
 53 |     private_nh.param("noise_var_pos", _noise_var_pos, 1e-4);
 54 |     ROS_INFO("noise_var_pos: %f", _noise_var_pos);
 55 |     private_nh.param("noise_var_yaw", _noise_var_yaw, 1e-4);
 56 |     ROS_INFO("noise_var_yaw: %f", _noise_var_yaw);
 57 |     private_nh.param("noise_var_vel", _noise_var_vel, 1e-4);
 58 |     ROS_INFO("noise_var_vel: %f", _noise_var_vel);
 59 |     private_nh.param("noise_var_meas", _noise_var_meas, 1e-4);
 60 |     ROS_INFO("noise_var_meas: %f", _noise_var_meas);
 61 |     private_nh.param("init_state_provided", _init_state_provided, false);
 62 |     ROS_INFO("init_state_provided: %d", _init_state_provided);
 63 |     private_nh.param("init_x", _init_x, 0.0);
 64 |     ROS_INFO("init_x: %f", _init_x);
 65 |     private_nh.param("init_y", _init_y, 0.0);
 66 |     ROS_INFO("init_y: %f", _init_y);
 67 |     private_nh.param("init_yaw", _init_yaw, 0.0);
 68 |     ROS_INFO("init_yaw: %f", _init_yaw);
 69 |     private_nh.param("init_velx", _init_velx, 0.0);
 70 |     ROS_INFO("init_velx: %f", _init_velx);
 71 |     private_nh.param("init_vely", _init_vely, 0.0);
 72 |     ROS_INFO("init_vely: %f", _init_vely);
 73 |     private_nh.param("init_var_pos", _init_var_pos, 1e-4);
 74 |     ROS_INFO("init_var_pos: %f", _init_var_pos);
 75 |     private_nh.param("init_var_yaw", _init_var_yaw, 1e-4);
 76 |     ROS_INFO("init_var_yaw: %f", _init_var_yaw);
 77 |     private_nh.param("init_var_vel", _init_var_vel, 1e-4);
 78 |     ROS_INFO("init_var_vel: %f", _init_var_vel);
 79 | 
 80 |     private_nh.param<std::string>("imu_topic", _imu_topic, "/imu/data");
 81 |     ROS_INFO("imu_topic: %s", _imu_topic.c_str());
 82 |     private_nh.param<std::string>("gnss_topic", _gnss_topic, "/fix");
 83 |     ROS_INFO("gnss_topic: %s", _gnss_topic.c_str());
 84 |     private_nh.param<std::string>("odom_topic", _odom_topic, "/odometry/car");
 85 |     ROS_INFO("odom_topic: %s", _odom_topic.c_str());
 86 | 
 87 |     imu_sub = node.subscribe(_imu_topic, 1, &UKF::imuCallback, this);
 88 |     gnss_sub = node.subscribe(_gnss_topic, 1, &UKF::gnssCallback, this);
 89 |     odom_pub = node.advertise<nav_msgs::Odometry>(_odom_topic, 1);
 90 | 
 91 |     //initializing values
 92 |     if(_init_state_provided)
 93 |     {
 94 |         _curr_state.vec(0) = _init_x;
 95 |         _curr_state.vec(1) = _init_y;
 96 |         _curr_state.vec(2) = _init_yaw;
 97 |         _curr_state.vec(3) = _init_velx;
 98 |         _curr_state.vec(4) = _init_vely;
 99 |         _curr_state.cov_mat.diagonal() << _init_var_pos, _init_var_pos, _init_var_yaw,
100 |                                          _init_var_vel, _init_var_vel;
101 |     }
102 | 
103 |     _process_noise_mat.diagonal() << _noise_var_pos, _noise_var_pos, _noise_var_yaw,
104 |                                      _noise_var_vel, _noise_var_vel;
105 | 
106 |     _meas_noise_mat.diagonal() << _noise_var_meas, _noise_var_meas;
107 | 
108 |     _is_first_imu = true;
109 |     _is_first_gnss = true;
110 |     _imu_available = false;
111 |     _gnss_available = false;
112 | 
113 |     _prev_imu_time = 0.0;
114 |     _curr_imu_time = 0.0;
115 | 
116 |     _init_utm = make_pair(0.0, 0.0);
117 |     _curr_utm = make_pair(0.0, 0.0);
118 |     _vp_at_first_gnss = make_pair(0.0, 0.0);
119 | 
120 |     _propagated_sigma_pt_mat = Matrix<double, 5, 11>::Zero();
121 | 
122 |     //cout<<"Constructor done"<<endl;
123 | }
124 | 
125 | /* @brief Callback for IMU data */
126 | void UKF::imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
127 | {
128 |     //cout<<"In imu callback"<<endl;
129 | 
130 |     if(_is_first_imu)
131 |     {
132 |         _prev_imu_time = msg->header.stamp.toSec();
133 |         _curr_imu_time = msg->header.stamp.toSec();
134 | 
135 |         if(!_init_state_provided)
136 |             initVehicleState(getYaw(msg->orientation));
137 | 
138 |         _is_first_imu = false; 
139 |     }
140 | 
141 |     else
142 |     {
143 |         _imu_available =true;
144 | 
145 |         _prev_imu_time = _curr_imu_time;
146 |         _curr_imu_time = msg->header.stamp.toSec();
147 | 
148 |         //fetching the motion model input
149 |         _mmi.ax = msg->linear_acceleration.x;
150 |         _mmi.ay = msg->linear_acceleration.y;
151 |         _mmi.omega = msg->angular_velocity.z;
152 |         _mmi.delT = _curr_imu_time - _prev_imu_time;
153 |     }
154 | 
155 |     return;
156 | }
157 | 
158 | /* @brief Callback for GNSS data */
159 | void UKF::gnssCallback(const sensor_msgs::NavSatFix::ConstPtr& fix)
160 | {
161 |     //cout<<"In gnss callback"<<endl;
162 |     
163 |     std::string zone;
164 |     
165 |     if(_is_first_gnss)
166 |     {
167 |         //converting fix to UTM 
168 |         //LLtoUTM arguments : (lat, long, northing, easting, zone)
169 |         gps_common::LLtoUTM(fix->latitude, fix->longitude, _init_utm.second, _init_utm.first, zone);
170 | 
171 |         //saving position of the vehicle when 1st GNSS reading is received
172 |         _vp_at_first_gnss.first = _curr_state.vec(0);
173 |         _vp_at_first_gnss.second = _curr_state.vec(1);
174 | 
175 |         cout<<"Pos at 1st gnss, x : "<<_curr_state.vec(0)<<", y : "<<_curr_state.vec(1)<<endl;
176 | 
177 |         _is_first_gnss = false;
178 |     }
179 | 
180 |     else
181 |     {
182 |         _gnss_available = true;
183 |         //converting fix to UTM 
184 |         //LLtoUTM arguments : (lat, long, northing, easting, zone)
185 |         gps_common::LLtoUTM(fix->latitude, fix->longitude, _curr_utm.second, _curr_utm.first, zone);
186 |     }
187 | 
188 |     return;
189 | }
190 | 
191 | /* @brief Initializes the Vehicle state to default values i.e., (0.0, 0.0, heading, 0.0, 0.0)
192 | where yaw is the initial yaw angle obtained from IMU when the program starts*/
193 | void UKF::initVehicleState(const double yaw)
194 | {
195 |     cout<<"Initializing vehicle state with yaw = "<<yaw<<endl;
196 |    
197 |     _curr_state.vec(2) = yaw;
198 | 
199 |     return;
200 | }
201 | 
202 | /* @brief Applying the motion model using acceleration and yaw rate data from IMU */
203 | Vector5d UKF::applyMotionModel(Vector5d vec)
204 | {
205 |     //cout<<"Applying motion model"<<endl;
206 |     
207 |     Vector5d propagated_vec;
208 | 
209 |     double del_theta = _mmi.omega*_mmi.delT;
210 |     Rotation2D<double> rot1(del_theta);
211 |     Vector2d del_vec(_mmi.ax*_mmi.delT, _mmi.ay*_mmi.delT);
212 |     propagated_vec(2) = vec(2) + del_theta; //updating yaw
213 |     //bounding yaw between -PI and PI
214 |     if(propagated_vec(2) > M_PI)
215 |         propagated_vec(2) = propagated_vec(2) - 2*M_PI;
216 |     else if(propagated_vec(2) <= -M_PI)
217 |         propagated_vec(2) = propagated_vec(2) + 2*M_PI;
218 | 
219 |     /* Transforming velocities in previous base_link frame to current one and add acc*delT */
220 |     propagated_vec.segment(3,2) = rot1.toRotationMatrix()*vec.segment(3,2) + del_vec;
221 |     
222 |     /* 
223 |     This might look like using predicted velocity rather than velocity at previous time instance, but it is
224 |     done to reduce computations, using v=u+at, x=ut+0.5at^2 is same as y=vt-0.5at^2
225 |     */
226 |     del_vec(0) = propagated_vec(3)*_mmi.delT - _mmi.ax*pow(_mmi.delT,2);
227 |     del_vec(1) = propagated_vec(4)*_mmi.delT - _mmi.ay*pow(_mmi.delT,2);
228 |     Rotation2D<double> rot2(propagated_vec(2));
229 |     propagated_vec.segment(0,2) = rot2.inverse().toRotationMatrix()*del_vec + vec.segment(0,2);
230 | 
231 |     return propagated_vec;
232 | }
233 | 
234 | /* @brief The prediction step */
235 | void UKF::predict()
236 | {
237 |     //cout<<"In prediction step"<<endl;
238 |     
239 |     Matrix5d L = _curr_state.cov_mat.llt().matrixL(); //Cholesky factor L
240 | 
241 |     //calculating sigma points, using N+k = 3.0 for Gaussian pdfs
242 |     _propagated_sigma_pt_mat.col(0) = applyMotionModel(_curr_state.vec);
243 |     for(int i=1;i<=5;i++)
244 |     {
245 |         _propagated_sigma_pt_mat.col(i) = applyMotionModel(_curr_state.vec + sqrt(3.0)*L.col(i-1));
246 |         _propagated_sigma_pt_mat.col(i+5) = applyMotionModel(_curr_state.vec - sqrt(3.0)*L.col(i-1));
247 |     }
248 | 
249 |     //computing predicted mean
250 |     _predicted_state.vec = (-2.0/3.0)*_propagated_sigma_pt_mat.col(0);
251 |     for(int i=1;i<11;i++)
252 |         _predicted_state.vec += (1.0/6.0)*_propagated_sigma_pt_mat.col(i);
253 | 
254 |     //computing predited covariance
255 |     Matrix<double, 5, 11> del_state_mat = _propagated_sigma_pt_mat.colwise() - _predicted_state.vec;
256 |     
257 |     _predicted_state.cov_mat = (-2.0/3.0)*(del_state_mat.col(0))*(del_state_mat.col(0).transpose());
258 |     for(int i=1;i<11;i++)
259 |         _predicted_state.cov_mat += (1.0/6.0)*(del_state_mat.col(i))*(del_state_mat.col(i).transpose());
260 |     
261 |     _predicted_state.cov_mat += _process_noise_mat; //adding process noise covariance
262 | 
263 |     return;        
264 | }
265 | 
266 | /* @brief The correction step */
267 | void UKF::correct(const double x, const double y)
268 | {
269 |     //cout<<"In correction step"<<endl;
270 |     
271 |     Matrix<double,2,11> predicted_meas_mat = _propagated_sigma_pt_mat.block<2,11>(0,0);
272 |     Vector2d predicted_meas_vec(_predicted_state.vec(0), _predicted_state.vec(1)); //since measurement is the position itself
273 |     Matrix<double,2,11> del_meas_mat = predicted_meas_mat.colwise() - predicted_meas_vec;
274 |     Matrix<double, 5, 11> del_state_mat = _propagated_sigma_pt_mat.colwise() - _predicted_state.vec;
275 | 
276 |     Matrix2d predicted_cov_mat;
277 |     Matrix<double,5,2> cross_cov_mat; //cross covariance matrix
278 | 
279 |     predicted_cov_mat = (-2.0/3.0)*del_meas_mat.col(0)*(del_meas_mat.col(0).transpose());
280 |     cross_cov_mat = (-2.0/3.0)*del_state_mat.col(0)*(del_meas_mat.col(0).transpose());
281 |     for(int i=1;i<11;i++)
282 |     {
283 |         predicted_cov_mat += (1.0/6.0)*del_meas_mat.col(i)*(del_meas_mat.col(i).transpose());
284 |         cross_cov_mat += (1.0/6.0)*del_state_mat.col(i)*(del_meas_mat.col(i).transpose());
285 |     }
286 |     
287 |     predicted_cov_mat += _meas_noise_mat; //adding measurement noise covariance
288 | 
289 |     Matrix<double,5,2> kalman_gain = cross_cov_mat*predicted_cov_mat.inverse();
290 | 
291 |     Vector2d meas_vec(x,y); //actual measurements
292 | 
293 |     _curr_state.vec = _predicted_state.vec + kalman_gain*(meas_vec - predicted_meas_vec);
294 |     _curr_state.cov_mat = _predicted_state.cov_mat - kalman_gain*predicted_cov_mat*(kalman_gain.transpose());
295 | 
296 |     return;
297 | }
298 | 
299 | /* @brief Publishes the vehicle state as a nav_msgs::Odometry message*/
300 | void UKF::publishVehicleState()
301 | {
302 |     //cout<<"Publishing vehicle state"<<endl;
303 |     
304 |     nav_msgs::Odometry vs_msg;
305 |     vs_msg.header.stamp = ros::Time::now();
306 |     vs_msg.header.frame_id = "map";
307 |     vs_msg.child_frame_id = "base_link";
308 | 
309 |     vs_msg.pose.pose.position.x = _curr_state.vec(0);
310 |     vs_msg.pose.pose.position.y = _curr_state.vec(1);
311 |     vs_msg.pose.pose.position.z = 0.0;
312 | 
313 |     Eigen::AngleAxisd rot_mat (_curr_state.vec(2), Eigen::Vector3d::UnitZ());
314 |     Eigen::Quaterniond quat(rot_mat); //rotation matrix stored as a quaternion
315 |     vs_msg.pose.pose.orientation.x = quat.x();
316 |     vs_msg.pose.pose.orientation.y = quat.y();
317 |     vs_msg.pose.pose.orientation.z = quat.z();
318 |     vs_msg.pose.pose.orientation.w = quat.w();
319 | 
320 |     vs_msg.twist.twist.linear.x = _curr_state.vec(3);
321 |     vs_msg.twist.twist.linear.y = _curr_state.vec(4);
322 |     vs_msg.twist.twist.linear.z = 0.0;
323 |     vs_msg.twist.twist.angular.x = 0.0;
324 |     vs_msg.twist.twist.angular.y = 0.0;
325 |     vs_msg.twist.twist.angular.z = _mmi.omega; //this is not estimated using UKF, so IMU reading is directly used
326 | 
327 |     for(int i=0;i<36;i++)
328 |     {
329 |         vs_msg.pose.covariance[i] = 1e-6; //a very small number instead of zero
330 |         vs_msg.twist.covariance[i] = 1e-6;
331 |     }
332 | 
333 |     vs_msg.pose.covariance[0] = _curr_state.cov_mat(0,0); //variance of x
334 |     vs_msg.pose.covariance[7] = _curr_state.cov_mat(1,1); //variance of y
335 |     vs_msg.pose.covariance[35] = _curr_state.cov_mat(2,2); //variance of theta
336 |     vs_msg.twist.covariance[0] = _curr_state.cov_mat(3,3); //variance of vx
337 |     vs_msg.twist.covariance[7] = _curr_state.cov_mat(4,4); //variance of vy
338 | 
339 |     odom_pub.publish(vs_msg); //publishing odometry msg
340 | 
341 |     return;
342 | }
343 | 
344 | /* @brief Runs the UKF estimation */
345 | void UKF::estimateVehicleState()
346 | {
347 |     //cout<<"Estimating state"<<endl;
348 |     
349 |     if(_imu_available)
350 |     {
351 |         predict(); //the prediction step
352 | 
353 |         _imu_available = false; //false until new imu reading is received
354 |     }
355 |     
356 |     if(_gnss_available)
357 |     {
358 |         double meas_x = (_curr_utm.first - _init_utm.first) + _vp_at_first_gnss.first;
359 |         double meas_y = (_curr_utm.second - _init_utm.second) + _vp_at_first_gnss.second;
360 | 
361 |         //cout<<"meas x : "<<meas_x<<" , meas y : "<<meas_y<<endl;
362 | 
363 |         correct(meas_x, meas_y); //correction step
364 | 
365 |         _gnss_available = false; //false until new gnss reading is received
366 |     }
367 | 
368 |     else
369 |         _curr_state = _predicted_state; //since no new measurements
370 | 
371 |     publishVehicleState(); //publishing the state
372 | 
373 |     return;
374 | }


--------------------------------------------------------------------------------