LDSC_Robotics_TAs
lib
Dependencies: mbed ros_lib_indigo
Diff: main.cpp
- Revision:
- 0:c94b2f0a4409
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Oct 02 11:30:47 2017 +0000
@@ -0,0 +1,2615 @@
+#include "mbed.h"
+#include "led.h"
+#include "uart_send.h"
+#include <ros.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Twist.h>
+#include "LSM9DS0_SH.h"
+#include <math.h>
+
+#define r_01 ((double)rand()/(RAND_MAX)) // uniform distribution 0 ~ 1
+#define constrain(amt,low,high) ((amt)<(low) ? (low) : ((amt) > (high) ? (high) : (amt)))
+
+// timer 1 : sensor fusion
+#define LOOPTIME 2000 // 0.002 s
+unsigned long lastMilli = 0;
+// sampling time
+float T = 0.002;
+
+// timer 2 : publish cmd_vel for mag_cor
+#define LOOPTIME2 100000 // 0.1 s
+unsigned long lastMilli2 = 0;
+
+// timer 3 : take part of mag data
+#define LOOPTIME3 300000 // 0.3 s
+unsigned long lastMilli3 = 0;
+
+// timer 5 : extended kalman filter
+#define LOOPTIME5 20000 // 0.02 s
+unsigned long lastMilli5 = 0;
+// sampling time
+float T2 = 0.02;
+
+// timer 6 : navigation
+#define LOOPTIME6 100000 // 0.1 s
+unsigned long lastMilli6 = 0;
+
+Timer t;
+
+ros::NodeHandle nh;
+
+geometry_msgs::Twist vel_msg;
+ros::Publisher p("andbot/cmd_vel", &vel_msg);
+unsigned long current_time = 0;
+unsigned long last_time = 0;
+
+// car information
+double wheelRadius = 0.078;
+double wheelSeparation = 0.393;
+double omega_right = 0.0;
+double omega_left = 0.0;
+double omega_z = 0.0;
+double vel_x = 0.0;
+double th = 0.0;
+int th_deg = 0;
+double x = 0.0;
+double y_ = 0.0;
+
+// start car rotation for magnetometer correction flag
+int check_flag = 0;
+
+// magnetometer correction data
+// magnetometer 1
+float magx[100] = {0};
+float magy[100] = {0};
+int ind = 0;
+int ind_ = 0;
+float cal_flag = 0;
+float magx_max = 0;
+float magx_min = 0;
+float magx0 = 0;
+float magy_max = 0;
+float magy_min = 0;
+float magy0 = 0;
+float a = 0.3324; // long axis
+float b = 0.3082; // short axis
+
+// magnetometer 2
+float magx2[100] = {0};
+float magy2[100] = {0};
+float magx_max2 = 0;
+float magx_min2 = 0;
+float magx02 = 0;
+float magy_max2 = 0;
+float magy_min2 = 0;
+float magy02 = 0;
+float a2 = 0.3366; // long axis
+float b2 = 0.3312; // short axis
+
+// vision data
+int px = 0;
+int py = 0;
+int vision_flag = 0;
+// calibration parameter
+float alpha_u = 179.869823;
+float alpha_v = 179.984858;
+float alpha_s = 0;
+float cx = 81.709410;
+float cy = 64.218079;
+// c : camera coordinate
+float Zc = 420; // unit : mm
+float Xc = 0;
+float Yc = 0;
+float Theta_c = 0;
+// r : robot coordinate
+float Xr = 0;
+float Yr = 0;
+// from odometry and cross detection to predict map coordinate
+float Xp = 0;
+float Yp = 0;
+
+// subscribe feedback_wheel_angularVel
+void messageCb(const geometry_msgs::Vector3& msg)
+{
+ Y_ = !Y_;
+
+ omega_left = msg.x;
+ omega_right = msg.y;
+
+ omega_z = wheelRadius * (omega_right - omega_left) / wheelSeparation;
+ vel_x = wheelRadius * (omega_right + omega_left) / 2;
+
+ current_time = t.read_ms();
+ double dt = (current_time - last_time) * 0.001; // from ms to s
+ last_time = current_time;
+
+ double delta_th = omega_z * dt;
+ th += delta_th;
+
+ double delta_x = vel_x * cos(th) * dt;
+ x += delta_x;
+
+ double delta_y = vel_x * sin(th) * dt;
+ y_ += delta_y;
+
+ // for mag_cor
+ if(check_flag == 1)
+ {
+ if(th >= 2*PI)
+ {
+ check_flag = 0;
+ cal_flag = 1;
+ vel_msg.angular.z = 0.0;
+ p.publish(&vel_msg);
+ th = 0;
+ }
+ }
+}
+
+ros::Subscriber<geometry_msgs::Vector3> s("feedback_wheel_angularVel",messageCb);
+
+int update_source = 0;
+
+// subscribe point
+void messageCb2(const geometry_msgs::Vector3& pp)
+{
+ px = pp.x;
+ py = pp.y;
+ Theta_c = pp.z;
+
+ if(px != 0 || py != 0 || Theta_c != 0)
+ vision_flag = 1;
+ else
+ vision_flag = 0;
+
+ if(vision_flag == 1 && px > 0 && py > 0)
+ {
+ // from image plane coordinate to camera coordinate
+ Xc = (Zc/alpha_u)*(px - cx) - ((alpha_s*Zc)/(alpha_u*alpha_v))*(py - cy);
+ Yc = (Zc/alpha_v)*(py - cy);
+
+ // from camera coordinate to robot coordinate
+ Xr = -Yc + 175;
+ Yr = -Xc - 10;
+
+ // change unit to meter
+ Xr = Xr * 0.001;
+ Yr = Yr * 0.001;
+
+ update_source = 1;
+ }
+ else if(vision_flag == 1 && px == 0 && py == 0 && ((Theta_c >= 1 && Theta_c <= 89) || (Theta_c >= -89 && Theta_c <= -1)))
+ {
+ update_source = 2;
+ }
+ else
+ {
+ Xr = 0;
+ Yr = 0;
+
+ update_source = 0;
+ }
+}
+
+ros::Subscriber<geometry_msgs::Vector3> s2("point",messageCb2);
+
+void read_sensor(void);
+
+float lpf(float input, float output_old, float frequency);
+
+int sensor_flag = 0; // sensor initial flag
+int sensor_count = 0;
+
+// sensor filter correction data
+float Gyro_axis_data_f[3];
+float Gyro_axis_data_f_old[3];
+float Gyro_scale[3]; // scale = (data - zero)
+float Gyro_axis_zero[3] = {0,0,0}; // x,y no use
+
+// sensor 1 (front)
+float Magn_axis_data_f[3];
+float Magn_axis_data_f_old[3];
+float Magn_scale[3]; // scale = (data - zero)
+float Magn_axis_zero[3] = {-0.1049,0.1982,0};
+
+// sensor 2 (back)
+float Magn_axis_data_f2[3];
+float Magn_axis_data_f_old2[3];
+float Magn_scale2[3]; // scale = (data - zero)
+float Magn_axis_zero2[3] = {0.1547,0.2713,0};
+
+float Bex = 0;
+float Bey = 0;
+float Bex2 = 0;
+float Bey2 = 0;
+float Be = 0.9882; // Be = ( sqrt(Bex*Bex + Bey*Bey) + sqrt(Bex2*Bex2 + Bey2*Bey2) ) / 2
+
+// counter for gyro bias
+int gyro_count = 0;
+int gyro_flag = 0;
+float gyro_init[10] = {0};
+int index_count = 0;
+float gyro_sum = 0;
+
+// sensor data
+// magn
+float phi = 0;
+float phi2 = 0;
+int phi_init_flag = 0;
+int phi_init_count = 0;
+float phi_init = 0;
+float phi_init2 = 0;
+float phi_m = 0;
+float phi_m2 = 0;
+int phi_m_deg = 0;
+int phi_m2_deg = 0;
+int phi_m_deg_1 = 0;
+int phi_m2_deg_1 = 0;
+int phi_m_deg_2 = 0;
+int phi_m2_deg_2 = 0;
+int phi_m_deg_T = 0;
+int phi_m2_deg_T = 0;
+float phi_m_a = 0;
+float phi_m2_a = 0;
+int phi_m_a_deg = 0;
+int phi_m2_a_deg = 0;
+int diff_phi_count = 0;
+int diff_phi_flag = 0;
+int jump_count = 0;
+int jump_count2 = 0;
+float phi_m_a_avg = 0;
+
+// gyro
+float w_s = 0;
+float w_s_d = 0;
+float theta_s = 0;
+int theta_s_deg = 0;
+
+// fusion data for theta
+float theta_fu = 0;
+float theta_fu_old = 0;
+float bias = 0;
+float bias_old = 0;
+float y = 0;
+float L1 = 0;
+float L2 = 0;
+int fusion_flag = 0;
+
+// theta_fu to ROS
+int theta_fu_deg = 0;
+int theta_fu_deg_180 = 0;
+float theta_fu_PI = 0;
+
+// add bias
+int bias_flag = 0;
+float bias_add = 0;
+
+// magnetic field disturbance detect
+// trust index
+float Q = 0;
+// Kalman filter
+float V = 0; // measurement noise variance
+float theta_me = 0;
+int theta_me_deg = 0;
+float theta_me_bar = 0;
+float sigma = 0;
+float sigma_bar = 0;
+float K = 0; // gain
+float At = 1;
+float Bt = 0.002; // sampling time
+float Ct = 1;
+float Rt = 0.001; // process noise variance
+float update_distance = 0;
+float update_rotation = 0;
+int update_flag = 0;
+
+// extended kalman filter for localization
+// EKF variables
+// prediction variables
+float mu_bar[3][1] = {0};
+int mu_bar_deg = 0;
+float mu[3][1] = {0}; // state vector
+float Sigma_bar[3][3] = {0};
+float Sigma[3][3] = { {0 , 0 , 0} , {0 , 0 , 0} , {0 , 0 , 0} }; // estimation covariance matrix
+float Sigma_mul_1[3][3] = {0};
+float Sigma_mul_2[3][3] = {0};
+float G[3][3] = {0}; // state transition matrix
+float GT[3][3] = {0}; // state transition matrix transport
+float R[3][3] = { {0.000001 , 0 , 0} , {0 , 0.000001 , 0} , {0 , 0 , 0.000007} }; // process noise covariance matrix
+
+// encoder odometry
+float th_enc = 0;
+int th_enc_deg = 0;
+float x_enc = 0;
+float y_enc = 0;
+
+// global cross map variables
+float block_length = 0.39; // m
+float x_crom[4] = {0};
+float y_crom[4] = {0};
+
+// update variables for cross detection
+float Zcro[2][1] = {0};
+float Zr[4] = {0};
+int Za[4] = {0};
+int Zphi[4] = {0};
+int Zphi_deg_180[4] = {0};
+float Zcro_hat[4][2][1] = {0};
+float Zd[4][2][1] = {0};
+float Pcro[4] = {0};
+float Pcro_max = 0;
+int i_Pcro_max = 0;
+float e_r[4] = {0};
+float mx_x[4] = {0};
+float my_y[4] = {0};
+float Zcro_hs[2][1] = {0};
+float Hcro[4][2][3] = {0};
+float Hcros[2][3] = {0};
+float HcroT[4][3][2] = {0};
+float HcroTs[3][2] = {0};
+float Qcro[2][2] = {0};
+float Scro[4][2][2] = {0};
+float det_Scro[4] = {0};
+float Scro_inv[4][2][2] = {0};
+float Scro_is[2][2] = {0};
+float Kcro[3][2] = {0};
+
+float Hcro_1[4][2][3] = {0};
+float Hcro_2[4][2][2] = {0};
+float Zcro_delta[2][1] = {0};
+float Kcro_1[3][2] = {0};
+float Kcro_2[3][1] = {0};
+float Icro_1[3][3] = {0};
+float Icro_2[3][3] = {0};
+
+// update variables for line detection
+float Zli = 0;
+float Zli_hat[4] = {0};
+float Zli_diff[4] = {0};
+float Zli_diff_min = 0;
+int ind_diff_min = 0;
+float Qli = 0.001;
+float Sli = 0;
+float Kli[3][1] = {0};
+
+// update occasion
+int camera_flag = 0;
+int update_state = 0;
+int singular_flag = 0;
+int predict_count = 0;
+int predict_flag = 0;
+float up_dis = 0;
+float up_rot = 0;
+int up_check = 0;
+float up_dis_r = 0.002;
+float up_rot_r = PI/60;
+
+// encoder magnetometer gyro odometry
+float th_emg = 0;
+int th_emg_deg = 0;
+float x_emg = 0;
+float y_emg = 0;
+
+// select update source for debugging
+// 0 for no update , 1 for cross update only , 2 for line update only , 3 for cross and line update together
+int select_ups = 3;
+
+// select prediction model for debugging
+// 0 for encoder magnetometer gyro fusion , 1 for encoder only
+int select_pms = 1;
+
+// navigation
+int mu_th_deg = 0;
+int mu_th_deg_180 = 0;
+float mu_th_PI = 0;
+float Pxd = 0;
+float Pyd = 0;
+float Od = 0;
+float delta_Pxd = 0;
+float delta_Pyd = 0;
+float delta_Od = 0;
+float Kv = 0.55;
+float Kw = 0.55;
+float gain_v = 0;
+float gain_w = 0;
+float error_P = 0;
+float error_O = 0;
+int nav_flag = 0;
+int nav_state = 1;
+float distance_error_P = 0;
+int line_tracker = 0;
+
+
+int main()
+{
+ t.start();
+
+ nh.initNode();
+
+ nh.subscribe(s);
+ nh.subscribe(s2);
+ nh.advertise(p);
+
+ init_uart();
+
+ init_IMU();
+
+ myled = 1;
+
+ while(1)
+ {
+ // receive labview data
+ if(uart_1.readable() > 0)
+ {
+ switch(uart_1.getc())
+ {
+ case 'a':
+ check_flag = 1;
+ BK_ = 0; // light
+ break;
+
+ case 's':
+ check_flag = 0;
+ BK_ = 1; // dark
+ break;
+
+ case 'z':
+ phi_init_flag = 1;
+ B_ = 0;
+ break;
+
+ case 'x':
+ phi_init_flag = 0;
+ B_ = 1;
+ break;
+
+ case 'b':
+ bias_flag = 1;
+ break;
+
+ case 'n':
+ bias_flag = 0;
+ break;
+
+ case 'c':
+ camera_flag = 1;
+ break;
+
+ case 'v':
+ camera_flag = 0;
+ break;
+
+ case 'h':
+ select_ups = 0;
+ break;
+
+ case 'j':
+ select_ups = 1;
+ break;
+
+ case 'k':
+ select_ups = 2;
+ break;
+
+ case 'l':
+ select_ups = 3;
+ break;
+
+ case 'y':
+ nav_flag = 1;
+ break;
+
+ case 'u':
+ nav_flag = 0;
+ break;
+
+ default:
+ check_flag = 0;
+ phi_init_flag = 0;
+ fusion_flag = 0;
+ bias_flag = 0;
+ camera_flag = 0;
+ select_ups = 0;
+ nav_flag = 0;
+ break;
+ }
+ }
+
+ // timer 1 : sensor fusion
+ if((t.read_us() - lastMilli) >= LOOPTIME) // 2000 us = 0.002 s
+ {
+ //myled = !myled;
+
+ lastMilli = t.read_us();
+
+ // sensor initial start
+ if(sensor_flag == 0)
+ {
+ sensor_count++;
+
+ if(sensor_count >= 100)
+ {
+ sensor_flag = 1;
+ sensor_count = 0;
+ }
+ }
+ else
+ {
+ read_sensor();
+
+////////////////// uart send data //////////////////////////////////////////////////////////////////////////////////////////////
+
+ display[0] = 1 * nav_flag;
+ display[1] = 1 * nav_state;
+ display[2] = 1 * up_check;
+ display[3] = 100 * error_P;
+ display[4] = 57.3 * error_O;
+ display[5] = 100 * distance_error_P;
+ display[6] = 100 * mu[0][0];
+ display[7] = 100 * mu[1][0];
+ display[8] = 57.3 * mu[2][0];
+ display[9] = 100 * x_enc;
+ display[10] = 100 * y_enc;
+ display[11] = 57.3 * th_enc;
+
+// display[0] = 1000 * Kcro_2[0][0];
+// display[1] = 1000 * Kcro_2[1][0];
+// display[2] = up_check;
+// display[3] = 100 * Zcro[0][0];
+// display[4] = 100 * Zcro_hat[i_Pcro_max][0][0];
+// display[5] = i_Pcro_max;
+// display[6] = 57.3 * Zcro[1][0];
+// display[7] = 57.3 * Zcro_hat[i_Pcro_max][1][0];
+// display[8] = Pcro_max;
+// display[9] = 100 * mu[0][0];
+// display[10] = 100 * mu[1][0];
+// display[11] = 57.3 * mu[2][0];
+
+// display[0] = 100 * x_crom[0];
+// display[1] = 100 * y_crom[0];
+// display[2] = i_Pcro_max;
+// display[3] = 100 * x_crom[1];
+// display[4] = 100 * y_crom[1];
+// display[5] = Pcro_max;
+// display[6] = 100 * x_crom[2];
+// display[7] = 100 * y_crom[2];
+// display[8] = 100 * Xp;
+// display[9] = 100 * x_crom[3];
+// display[10] = 100 * y_crom[3];
+// display[11] = 100 * Yp;
+
+// display[0] = 10000 * Magn_axis_zero[0];
+// display[1] = 10000 * Magn_axis_zero[1];
+// display[2] = 10000 * a;
+// display[3] = 10000 * b;
+// display[4] = 10000 * Magn_axis_zero2[0];
+// display[5] = 10000 * Magn_axis_zero2[1];
+// display[6] = 10000 * a2;
+// display[7] = 10000 * b2;
+// display[8] = 10000 * Be;
+// display[9] = 100 * Xr;
+// display[10] = 100 * Yr;
+// display[11] = 57.3 * th_enc;
+
+// display[0] = 57.3 * phi_m_a;
+// display[1] = 57.3 * phi_m2_a;
+// display[2] = 57.3 * (0.5*phi_m_a + 0.5*phi_m2_a);
+// display[3] = 57.3 * theta_fu;
+// display[4] = 57.3 * theta_s;
+// display[5] = 10000 * Q;
+// display[6] = 57.3 * theta_me;
+// display[7] = 57.3 * th_enc;
+// display[8] = 57.3 * phi_m_a_avg;
+// display[9] = 0;
+// display[10] = 0;
+// display[11] = 0;
+
+////////////////// uart send data //////////////////////////////////////////////////////////////////////////////////////////////
+
+ uart_send(display);
+ }
+ }
+
+ nh.spinOnce();
+
+ // timer 2 : publish cmd_vel for mag_cor
+ if((t.read_us() - lastMilli2) >= LOOPTIME2) // 100000 us = 0.1 s
+ {
+ lastMilli2 = t.read_us();
+
+ if(check_flag == 1)
+ {
+ vel_msg.angular.z = 0.35;
+ p.publish(&vel_msg);
+ }
+ }
+
+ // timer 3 : take part of mag data
+ if((t.read_us() - lastMilli3) >= LOOPTIME3) // 300000 us = 0.3 s
+ {
+ lastMilli3 = t.read_us();
+
+ if(check_flag == 1)
+ {
+ if(th >= PI/12) // after magn data stable then take data
+ {
+ magx[ind] = Magn_axis_data_f[0];
+ magy[ind] = Magn_axis_data_f[1];
+
+ magx2[ind] = Magn_axis_data_f2[0];
+ magy2[ind] = Magn_axis_data_f2[1];
+
+ ind++;
+ if(ind >= 100)
+ ind = 0;
+
+ ind_ = ind - 1; // send data index
+ }
+ }
+ }
+
+////////// timer 5 : extended kalman filter ////////////////////////////////////////////////////////////////////////////////////
+
+ if((t.read_us() - lastMilli5) >= LOOPTIME5) // 20000 us = 0.02 s
+ {
+ myled = !myled;
+
+ lastMilli5 = t.read_us();
+
+ if(fusion_flag == 1)
+ {
+ // extended kalman filter for robot localization
+
+ // *** Prediction Start *************************************************************** //
+
+ // encoder odometry
+ th_enc = th_enc + omega_z*T2;
+ x_enc = x_enc + vel_x*cos(th_enc)*T2;
+ y_enc = y_enc + vel_x*sin(th_enc)*T2;
+
+ // th_enc_deg range 0~359 deg
+ th_enc_deg = th_enc * 180/PI;
+
+ if(th_enc_deg < 0)
+ th_enc_deg = th_enc_deg % 360 + 360;
+ else
+ th_enc_deg = th_enc_deg % 360;
+
+ // encoder magnetometer gyro odometry
+ th_emg = theta_fu;
+ x_emg = x_emg + vel_x*cos(th_emg)*T2;
+ y_emg = y_emg + vel_x*sin(th_emg)*T2;
+
+ // th_emg_deg range 0~359 deg
+ th_emg_deg = th_emg * 180/PI;
+
+ if(th_emg_deg < 0)
+ th_emg_deg = th_emg_deg % 360 + 360;
+ else
+ th_emg_deg = th_emg_deg % 360;
+
+ // update distance and rotation angle
+ up_dis = up_dis + T2*vel_x;
+ up_rot = up_rot + T2*omega_z;
+
+ // mu_bar = g(u,mu)
+ switch(select_pms)
+ {
+ // 0 for encoder magnetometer gyro fusion
+ case 0:
+ mu_bar[0][0] = mu[0][0] + vel_x*cos(mu[2][0])*T2;
+ mu_bar[1][0] = mu[1][0] + vel_x*sin(mu[2][0])*T2;
+ mu_bar[2][0] = theta_fu;
+
+ mu_bar_deg = theta_fu_deg;
+ break;
+
+ // 1 for encoder only
+ case 1:
+ mu_bar[0][0] = mu[0][0] + vel_x*cos(mu[2][0])*T2;
+ mu_bar[1][0] = mu[1][0] + vel_x*sin(mu[2][0])*T2;
+ mu_bar[2][0] = mu[2][0] + omega_z*T2;
+
+ // mu_bar_deg range 0~359 deg
+ mu_bar_deg = mu_bar[2][0] * 180/PI;
+
+ if(mu_bar_deg < 0)
+ mu_bar_deg = mu_bar_deg % 360 + 360;
+ else
+ mu_bar_deg = mu_bar_deg % 360;
+ break;
+ }
+
+ // covariance prediction accumulation timing
+ if(vel_x != 0 || omega_z != 0)
+ {
+ predict_count++;
+ }
+
+ if(predict_count >= 25)
+ {
+ predict_flag = 1;
+
+ predict_count = 0;
+ }
+
+ if(predict_flag == 1)
+ {
+ // Sigma_bar = G*Sigma*G^T + R
+ G[0][0] = 1;
+ G[1][1] = 1;
+ G[2][2] = 1;
+ G[0][2] = -vel_x*sin(mu[2][0])*T2;
+ G[1][2] = vel_x*cos(mu[2][0])*T2;
+
+ // get GT
+ GT[0][0] = 1;
+ GT[1][1] = 1;
+ GT[2][2] = 1;
+ GT[2][0] = G[0][2];
+ GT[2][1] = G[1][2];
+
+ // G*Sigma = Sigma_mul_1
+ Sigma_mul_1[0][0] = G[0][0]*Sigma[0][0] + G[0][1]*Sigma[1][0] + G[0][2]*Sigma[2][0];
+ Sigma_mul_1[0][1] = G[0][0]*Sigma[0][1] + G[0][1]*Sigma[1][1] + G[0][2]*Sigma[2][1];
+ Sigma_mul_1[0][2] = G[0][0]*Sigma[0][2] + G[0][1]*Sigma[1][2] + G[0][2]*Sigma[2][2];
+ Sigma_mul_1[1][0] = G[1][0]*Sigma[0][0] + G[1][1]*Sigma[1][0] + G[1][2]*Sigma[2][0];
+ Sigma_mul_1[1][1] = G[1][0]*Sigma[0][1] + G[1][1]*Sigma[1][1] + G[1][2]*Sigma[2][1];
+ Sigma_mul_1[1][2] = G[1][0]*Sigma[0][2] + G[1][1]*Sigma[1][2] + G[1][2]*Sigma[2][2];
+ Sigma_mul_1[2][0] = G[2][0]*Sigma[0][0] + G[2][1]*Sigma[1][0] + G[2][2]*Sigma[2][0];
+ Sigma_mul_1[2][1] = G[2][0]*Sigma[0][1] + G[2][1]*Sigma[1][1] + G[2][2]*Sigma[2][1];
+ Sigma_mul_1[2][2] = G[2][0]*Sigma[0][2] + G[2][1]*Sigma[1][2] + G[2][2]*Sigma[2][2];
+
+ // G*Sigma * GT = Sigma_mul_1 * GT = Sigma_mul_2
+ Sigma_mul_2[0][0] = Sigma_mul_1[0][0]*GT[0][0] + Sigma_mul_1[0][1]*GT[1][0] + Sigma_mul_1[0][2]*GT[2][0];
+ Sigma_mul_2[0][1] = Sigma_mul_1[0][0]*GT[0][1] + Sigma_mul_1[0][1]*GT[1][1] + Sigma_mul_1[0][2]*GT[2][1];
+ Sigma_mul_2[0][2] = Sigma_mul_1[0][0]*GT[0][2] + Sigma_mul_1[0][1]*GT[1][2] + Sigma_mul_1[0][2]*GT[2][2];
+ Sigma_mul_2[1][0] = Sigma_mul_1[1][0]*GT[0][0] + Sigma_mul_1[1][1]*GT[1][0] + Sigma_mul_1[1][2]*GT[2][0];
+ Sigma_mul_2[1][1] = Sigma_mul_1[1][0]*GT[0][1] + Sigma_mul_1[1][1]*GT[1][1] + Sigma_mul_1[1][2]*GT[2][1];
+ Sigma_mul_2[1][2] = Sigma_mul_1[1][0]*GT[0][2] + Sigma_mul_1[1][1]*GT[1][2] + Sigma_mul_1[1][2]*GT[2][2];
+ Sigma_mul_2[2][0] = Sigma_mul_1[2][0]*GT[0][0] + Sigma_mul_1[2][1]*GT[1][0] + Sigma_mul_1[2][2]*GT[2][0];
+ Sigma_mul_2[2][1] = Sigma_mul_1[2][0]*GT[0][1] + Sigma_mul_1[2][1]*GT[1][1] + Sigma_mul_1[2][2]*GT[2][1];
+ Sigma_mul_2[2][2] = Sigma_mul_1[2][0]*GT[0][2] + Sigma_mul_1[2][1]*GT[1][2] + Sigma_mul_1[2][2]*GT[2][2];
+
+ // G*Sigma*GT + R = Sigma_mul_2 + R = Sigma_bar
+ Sigma_bar[0][0] = Sigma_mul_2[0][0] + R[0][0];
+ Sigma_bar[0][1] = Sigma_mul_2[0][1] + R[0][1];
+ Sigma_bar[0][2] = Sigma_mul_2[0][2] + R[0][2];
+ Sigma_bar[1][0] = Sigma_mul_2[1][0] + R[1][0];
+ Sigma_bar[1][1] = Sigma_mul_2[1][1] + R[1][1];
+ Sigma_bar[1][2] = Sigma_mul_2[1][2] + R[1][2];
+ Sigma_bar[2][0] = Sigma_mul_2[2][0] + R[2][0];
+ Sigma_bar[2][1] = Sigma_mul_2[2][1] + R[2][1];
+ Sigma_bar[2][2] = Sigma_mul_2[2][2] + R[2][2];
+
+ predict_flag = 0;
+ }
+
+ // *** Prediction End ***************************************************************** //
+
+ // *** Update Start ******************************************************************* //
+
+ if(vision_flag == 1 && camera_flag == 1)
+ {
+ if(update_source == 1 && singular_flag == 0) // cross detection
+ {
+ // global cross map
+ Xp = mu_bar[0][0] + cos(mu_bar[2][0])*Xr - sin(mu_bar[2][0])*Yr;
+ Yp = mu_bar[1][0] + sin(mu_bar[2][0])*Xr + cos(mu_bar[2][0])*Yr;
+
+ if(Xp >= 0 && Yp >= 0)
+ {
+ // left down (0,0)
+ x_crom[0] = ((int)(Xp/block_length) + 0) * block_length;
+ y_crom[0] = ((int)(Yp/block_length) + 0) * block_length;
+ // right down (1,0)
+ x_crom[1] = ((int)(Xp/block_length) + 1) * block_length;
+ y_crom[1] = ((int)(Yp/block_length) + 0) * block_length;
+ // right up (1,1)
+ x_crom[2] = ((int)(Xp/block_length) + 1) * block_length;
+ y_crom[2] = ((int)(Yp/block_length) + 1) * block_length;
+ // left up (0,1)
+ x_crom[3] = ((int)(Xp/block_length) + 0) * block_length;
+ y_crom[3] = ((int)(Yp/block_length) + 1) * block_length;
+ }
+ else if(Xp < 0 && Yp >= 0)
+ {
+ // left down (-1,0)
+ x_crom[0] = ((int)(Xp/block_length) - 1) * block_length;
+ y_crom[0] = ((int)(Yp/block_length) + 0) * block_length;
+ // right down (0,0)
+ x_crom[1] = ((int)(Xp/block_length) + 0) * block_length;
+ y_crom[1] = ((int)(Yp/block_length) + 0) * block_length;
+ // right up (0,1)
+ x_crom[2] = ((int)(Xp/block_length) + 0) * block_length;
+ y_crom[2] = ((int)(Yp/block_length) + 1) * block_length;
+ // left up (-1,1)
+ x_crom[3] = ((int)(Xp/block_length) - 1) * block_length;
+ y_crom[3] = ((int)(Yp/block_length) + 1) * block_length;
+ }
+ else if(Xp < 0 && Yp < 0)
+ {
+ // left down (-1,-1)
+ x_crom[0] = ((int)(Xp/block_length) - 1) * block_length;
+ y_crom[0] = ((int)(Yp/block_length) - 1) * block_length;
+ // right down (0,-1)
+ x_crom[1] = ((int)(Xp/block_length) + 0) * block_length;
+ y_crom[1] = ((int)(Yp/block_length) - 1) * block_length;
+ // right up (0,0)
+ x_crom[2] = ((int)(Xp/block_length) + 0) * block_length;
+ y_crom[2] = ((int)(Yp/block_length) + 0) * block_length;
+ // left up (-1,0)
+ x_crom[3] = ((int)(Xp/block_length) - 1) * block_length;
+ y_crom[3] = ((int)(Yp/block_length) + 0) * block_length;
+ }
+ else if(Xp >= 0 && Yp < 0)
+ {
+ // left down (0,-1)
+ x_crom[0] = ((int)(Xp/block_length) + 0) * block_length;
+ y_crom[0] = ((int)(Yp/block_length) - 1) * block_length;
+ // right down (1,-1)
+ x_crom[1] = ((int)(Xp/block_length) + 1) * block_length;
+ y_crom[1] = ((int)(Yp/block_length) - 1) * block_length;
+ // right up (1,0)
+ x_crom[2] = ((int)(Xp/block_length) + 1) * block_length;
+ y_crom[2] = ((int)(Yp/block_length) + 0) * block_length;
+ // left up (0,0)
+ x_crom[3] = ((int)(Xp/block_length) + 0) * block_length;
+ y_crom[3] = ((int)(Yp/block_length) + 0) * block_length;
+ }
+
+ // cross relative distance measurement , polar coordinates : (Zcro[0][0] , Zcro[1][0])
+ Zcro[0][0] = sqrt(pow(Xr,2) + pow(Yr,2));
+ Zcro[1][0] = atan2(Yr,Xr);
+
+ // cross relative distance prediction , polar coordinates : (Zr[i] , Zphi[i])
+ for(int i = 0 ; i < 4 ; i++)
+ {
+ Zr[i] = sqrt(pow((x_crom[i] - mu_bar[0][0]),2) + pow((y_crom[i] - mu_bar[1][0]),2));
+
+ Za[i] = atan2((y_crom[i] - mu_bar[1][0]),(x_crom[i] - mu_bar[0][0])) * 180/PI;
+
+ // Za[i] range 0~359 deg
+ if(Za[i] < 0)
+ Za[i] = Za[i] % 360 + 360;
+ else
+ Za[i] = Za[i] % 360;
+
+ Zphi[i] = Za[i] - mu_bar_deg;
+
+ // Zphi[i] range 0~359 deg
+ if(Zphi[i] < 0)
+ Zphi[i] = Zphi[i] % 360 + 360;
+ else
+ Zphi[i] = Zphi[i] % 360;
+
+ // Zphi_deg_180[i] range -179~180 deg
+ if(Zphi[i] > 180)
+ Zphi_deg_180[i] = Zphi[i] - 360;
+ else
+ Zphi_deg_180[i] = Zphi[i];
+
+ Zcro_hat[i][0][0] = Zr[i];
+ Zcro_hat[i][1][0] = Zphi_deg_180[i] * PI/180;
+ }
+
+ // cross measurement likelihood
+ for(int i = 0 ; i < 4 ; i++)
+ {
+ // measurement transition matrix element
+ if(Zr[i] != 0)
+ {
+ e_r[i] = 1 / Zr[i];
+
+ singular_flag = 0;
+ }
+ else
+ {
+ singular_flag = 1;
+ }
+
+ mx_x[i] = x_crom[i] - mu_bar[0][0];
+ my_y[i] = y_crom[i] - mu_bar[1][0];
+
+ Hcro[i][0][0] = -mx_x[i] * e_r[i];
+ Hcro[i][1][0] = my_y[i] * pow(e_r[i],2);
+ Hcro[i][0][1] = -my_y[i] * e_r[i];
+ Hcro[i][1][1] = -mx_x[i] * pow(e_r[i],2);
+ Hcro[i][0][2] = 0;
+ Hcro[i][1][2] = -1;
+
+ // get HcroT
+ HcroT[i][0][0] = Hcro[i][0][0];
+ HcroT[i][1][0] = Hcro[i][0][1];
+ HcroT[i][2][0] = Hcro[i][0][2];
+ HcroT[i][0][1] = Hcro[i][1][0];
+ HcroT[i][1][1] = Hcro[i][1][1];
+ HcroT[i][2][1] = Hcro[i][1][2];
+
+ // H*Sigma_bar = Hcro_1
+ Hcro_1[i][0][0] = Hcro[i][0][0]*Sigma_bar[0][0] + Hcro[i][0][1]*Sigma_bar[1][0] + Hcro[i][0][2]*Sigma_bar[2][0];
+ Hcro_1[i][0][1] = Hcro[i][0][0]*Sigma_bar[0][1] + Hcro[i][0][1]*Sigma_bar[1][1] + Hcro[i][0][2]*Sigma_bar[2][1];
+ Hcro_1[i][0][2] = Hcro[i][0][0]*Sigma_bar[0][2] + Hcro[i][0][1]*Sigma_bar[1][2] + Hcro[i][0][2]*Sigma_bar[2][2];
+ Hcro_1[i][1][0] = Hcro[i][1][0]*Sigma_bar[0][0] + Hcro[i][1][1]*Sigma_bar[1][0] + Hcro[i][1][2]*Sigma_bar[2][0];
+ Hcro_1[i][1][1] = Hcro[i][1][0]*Sigma_bar[0][1] + Hcro[i][1][1]*Sigma_bar[1][1] + Hcro[i][1][2]*Sigma_bar[2][1];
+ Hcro_1[i][1][2] = Hcro[i][1][0]*Sigma_bar[0][2] + Hcro[i][1][1]*Sigma_bar[1][2] + Hcro[i][1][2]*Sigma_bar[2][2];
+
+ // H*Sigma_bar * HT = Hcro_1 * HT = Hcro_2
+ Hcro_2[i][0][0] = Hcro_1[i][0][0]*HcroT[i][0][0] + Hcro_1[i][0][1]*HcroT[i][1][0] + Hcro_1[i][0][2]*HcroT[i][2][0];
+ Hcro_2[i][0][1] = Hcro_1[i][0][0]*HcroT[i][0][1] + Hcro_1[i][0][1]*HcroT[i][1][1] + Hcro_1[i][0][2]*HcroT[i][2][1];
+ Hcro_2[i][1][0] = Hcro_1[i][1][0]*HcroT[i][0][0] + Hcro_1[i][1][1]*HcroT[i][1][0] + Hcro_1[i][1][2]*HcroT[i][2][0];
+ Hcro_2[i][1][1] = Hcro_1[i][1][0]*HcroT[i][0][1] + Hcro_1[i][1][1]*HcroT[i][1][1] + Hcro_1[i][1][2]*HcroT[i][2][1];
+
+ Qcro[0][0] = 0.0001;
+ Qcro[1][1] = 0.001;
+
+ // H*Sigma_bar*HT + Q = Hcro_2 + Q = Scro
+ Scro[i][0][0] = Hcro_2[i][0][0] + Qcro[0][0];
+ Scro[i][0][1] = Hcro_2[i][0][1] + Qcro[0][1];
+ Scro[i][1][0] = Hcro_2[i][1][0] + Qcro[1][0];
+ Scro[i][1][1] = Hcro_2[i][1][1] + Qcro[1][1];
+
+ det_Scro[i] = Scro[i][0][0]*Scro[i][1][1] - Scro[i][0][1]*Scro[i][1][0];
+
+ if(det_Scro[i] != 0)
+ {
+ Scro_inv[i][0][0] = Scro[i][1][1] / det_Scro[i];
+ Scro_inv[i][0][1] = -Scro[i][0][1] / det_Scro[i];
+ Scro_inv[i][1][0] = -Scro[i][1][0] / det_Scro[i];
+ Scro_inv[i][1][1] = Scro[i][0][0] / det_Scro[i];
+
+ singular_flag = 0;
+ }
+ else
+ {
+ singular_flag = 1;
+ }
+
+ // measurement difference
+ Zd[i][0][0] = Zcro[0][0] - Zcro_hat[i][0][0];
+ Zd[i][1][0] = Zcro[1][0] - Zcro_hat[i][1][0];
+
+ // measurement likelihood
+ Pcro[i] = (1/sqrt(4*PI*PI*det_Scro[i]))*exp(-0.5*((Zd[i][0][0]*Scro_inv[i][0][0]+Zd[i][1][0]*Scro_inv[i][1][0])*Zd[i][0][0]+(Zd[i][0][0]*Scro[i][0][1]+Zd[i][1][0]*Scro_inv[i][1][1])*Zd[i][1][0]));
+ }
+
+ // find the maximum likelihood and index
+ Pcro_max = Pcro[0];
+ i_Pcro_max = 0;
+
+ for(int j = 1 ; j < 4 ; j++)
+ {
+ if(Pcro[j] > Pcro_max)
+ {
+ Pcro_max = Pcro[j];
+ i_Pcro_max = j;
+ }
+ }
+
+ // Z_hat select
+ Zcro_hs[0][0] = Zcro_hat[i_Pcro_max][0][0];
+ Zcro_hs[1][0] = Zcro_hat[i_Pcro_max][1][0];
+
+ // H select
+ Hcros[0][0] = Hcro[i_Pcro_max][0][0];
+ Hcros[0][1] = Hcro[i_Pcro_max][0][1];
+ Hcros[0][2] = Hcro[i_Pcro_max][0][2];
+ Hcros[1][0] = Hcro[i_Pcro_max][1][0];
+ Hcros[1][1] = Hcro[i_Pcro_max][1][1];
+ Hcros[1][2] = Hcro[i_Pcro_max][1][2];
+
+ // HT select
+ HcroTs[0][0] = HcroT[i_Pcro_max][0][0];
+ HcroTs[0][1] = HcroT[i_Pcro_max][0][1];
+ HcroTs[1][0] = HcroT[i_Pcro_max][1][0];
+ HcroTs[1][1] = HcroT[i_Pcro_max][1][1];
+ HcroTs[2][0] = HcroT[i_Pcro_max][2][0];
+ HcroTs[2][1] = HcroT[i_Pcro_max][2][1];
+
+ // S^-1 select
+ Scro_is[0][0] = Scro_inv[i_Pcro_max][0][0];
+ Scro_is[0][1] = Scro_inv[i_Pcro_max][0][1];
+ Scro_is[1][0] = Scro_inv[i_Pcro_max][1][0];
+ Scro_is[1][1] = Scro_inv[i_Pcro_max][1][1];
+
+ // get (Z-Z_hat)
+ Zcro_delta[0][0] = Zcro[0][0] - Zcro_hs[0][0];
+ Zcro_delta[1][0] = Zcro[1][0] - Zcro_hs[1][0];
+
+ // Sigma_bar*HT = Kcro_1
+ Kcro_1[0][0] = Sigma_bar[0][0]*HcroTs[0][0] + Sigma_bar[0][1]*HcroTs[1][0] + Sigma_bar[0][2]*HcroTs[2][0];
+ Kcro_1[0][1] = Sigma_bar[0][0]*HcroTs[0][1] + Sigma_bar[0][1]*HcroTs[1][1] + Sigma_bar[0][2]*HcroTs[2][1];
+ Kcro_1[1][0] = Sigma_bar[1][0]*HcroTs[0][0] + Sigma_bar[1][1]*HcroTs[1][0] + Sigma_bar[1][2]*HcroTs[2][0];
+ Kcro_1[1][1] = Sigma_bar[1][0]*HcroTs[0][1] + Sigma_bar[1][1]*HcroTs[1][1] + Sigma_bar[1][2]*HcroTs[2][1];
+ Kcro_1[2][0] = Sigma_bar[2][0]*HcroTs[0][0] + Sigma_bar[2][1]*HcroTs[1][0] + Sigma_bar[2][2]*HcroTs[2][0];
+ Kcro_1[2][1] = Sigma_bar[2][0]*HcroTs[0][1] + Sigma_bar[2][1]*HcroTs[1][1] + Sigma_bar[2][2]*HcroTs[2][1];
+
+ // Sigma_bar*HT * S^-1 = Kcro_1 * S^-1 = Kcro
+ Kcro[0][0] = Kcro_1[0][0]*Scro_is[0][0] + Kcro_1[0][1]*Scro_is[1][0];
+ Kcro[0][1] = Kcro_1[0][0]*Scro_is[0][1] + Kcro_1[0][1]*Scro_is[1][1];
+ Kcro[1][0] = Kcro_1[1][0]*Scro_is[0][0] + Kcro_1[1][1]*Scro_is[1][0];
+ Kcro[1][1] = Kcro_1[1][0]*Scro_is[0][1] + Kcro_1[1][1]*Scro_is[1][1];
+ Kcro[2][0] = Kcro_1[2][0]*Scro_is[0][0] + Kcro_1[2][1]*Scro_is[1][0];
+ Kcro[2][1] = Kcro_1[2][0]*Scro_is[0][1] + Kcro_1[2][1]*Scro_is[1][1];
+
+ // K*(Z-Z_hat) = Kcro_2
+ Kcro_2[0][0] = Kcro[0][0]*Zcro_delta[0][0] + Kcro[0][1]*Zcro_delta[1][0];
+ Kcro_2[1][0] = Kcro[1][0]*Zcro_delta[0][0] + Kcro[1][1]*Zcro_delta[1][0];
+ Kcro_2[2][0] = Kcro[2][0]*Zcro_delta[0][0] + Kcro[2][1]*Zcro_delta[1][0];
+
+ // K*H = Icro_1
+ Icro_1[0][0] = Kcro[0][0]*Hcros[0][0] + Kcro[0][1]*Hcros[1][0];
+ Icro_1[0][1] = Kcro[0][0]*Hcros[0][1] + Kcro[0][1]*Hcros[1][1];
+ Icro_1[0][2] = Kcro[0][0]*Hcros[0][2] + Kcro[0][1]*Hcros[1][2];
+ Icro_1[1][0] = Kcro[1][0]*Hcros[0][0] + Kcro[1][1]*Hcros[1][0];
+ Icro_1[1][1] = Kcro[1][0]*Hcros[0][1] + Kcro[1][1]*Hcros[1][1];
+ Icro_1[1][2] = Kcro[1][0]*Hcros[0][2] + Kcro[1][1]*Hcros[1][2];
+ Icro_1[2][0] = Kcro[2][0]*Hcros[0][0] + Kcro[2][1]*Hcros[1][0];
+ Icro_1[2][1] = Kcro[2][0]*Hcros[0][1] + Kcro[2][1]*Hcros[1][1];
+ Icro_1[2][2] = Kcro[2][0]*Hcros[0][2] + Kcro[2][1]*Hcros[1][2];
+
+ // (I - K*H) = (I - Icro_1) = Icro_2
+ Icro_2[0][0] = 1 - Icro_1[0][0];
+ Icro_2[0][1] = 0 - Icro_1[0][1];
+ Icro_2[0][2] = 0 - Icro_1[0][2];
+ Icro_2[1][0] = 0 - Icro_1[1][0];
+ Icro_2[1][1] = 1 - Icro_1[1][1];
+ Icro_2[1][2] = 0 - Icro_1[1][2];
+ Icro_2[2][0] = 0 - Icro_1[2][0];
+ Icro_2[2][1] = 0 - Icro_1[2][1];
+ Icro_2[2][2] = 1 - Icro_1[2][2];
+
+ update_state = 1;
+ }
+ // if(update_source == 1) end
+ else if(update_source == 2) // line detection
+ {
+ // line angle measurement
+ Zli = Theta_c;
+
+ // measurement prediction covariance
+ Sli = Sigma_bar[2][2] + Qli;
+
+ // measurement likelihood (simplified method)
+ Zli_hat[0] = mu_bar_deg - 90;
+ Zli_hat[1] = mu_bar_deg - 180;
+ Zli_hat[2] = mu_bar_deg - 270;
+ Zli_hat[3] = mu_bar_deg - 360;
+ if(Zli_hat[3] < -270)
+ Zli_hat[3] = Zli_hat[3] + 360;
+
+ // measurement difference square
+ Zli_diff[0] = (Zli - Zli_hat[0]) * (Zli - Zli_hat[0]);
+ Zli_diff[1] = (Zli - Zli_hat[1]) * (Zli - Zli_hat[1]);
+ Zli_diff[2] = (Zli - Zli_hat[2]) * (Zli - Zli_hat[2]);
+ Zli_diff[3] = (Zli - Zli_hat[3]) * (Zli - Zli_hat[3]);
+
+ // find the minimum measurement difference square and index
+ Zli_diff_min = Zli_diff[0];
+ ind_diff_min = 0;
+
+ for(int j = 1 ; j < 4 ; j++)
+ {
+ if(Zli_diff[j] < Zli_diff_min)
+ {
+ Zli_diff_min = Zli_diff[j];
+ ind_diff_min = j;
+ }
+ }
+
+ if(Zli_diff_min <= 150)
+ {
+ // Kli = Sigma_bar * HT * S^-1
+ Kli[2][0] = Sigma_bar[2][2] / Sli;
+
+ update_state = 2;
+ }
+ else
+ {
+ update_state = 0;
+ }
+ }
+ // else if(update_source == 2) end
+ else
+ {
+ update_state = 0;
+ }
+ }
+
+ // *** Update End ********************************************************************* //
+
+ if(up_dis >= up_dis_r)
+ {
+ // update
+ if(update_state == 1)
+ {
+ switch(select_ups)
+ {
+ // 0 for no update
+ case 0:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 1 for cross update only
+ case 1:
+ // mu_bar + K*(Z-Z_hat) = mu
+ mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
+ mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
+
+ // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
+ Sigma_bar[0][0] = Icro_2[0][0]*Sigma_bar[0][0] + Icro_2[0][1]*Sigma_bar[1][0] + Icro_2[0][2]*Sigma_bar[2][0];
+ Sigma_bar[0][1] = Icro_2[0][0]*Sigma_bar[0][1] + Icro_2[0][1]*Sigma_bar[1][1] + Icro_2[0][2]*Sigma_bar[2][1];
+ Sigma_bar[0][2] = Icro_2[0][0]*Sigma_bar[0][2] + Icro_2[0][1]*Sigma_bar[1][2] + Icro_2[0][2]*Sigma_bar[2][2];
+ Sigma_bar[1][0] = Icro_2[1][0]*Sigma_bar[0][0] + Icro_2[1][1]*Sigma_bar[1][0] + Icro_2[1][2]*Sigma_bar[2][0];
+ Sigma_bar[1][1] = Icro_2[1][0]*Sigma_bar[0][1] + Icro_2[1][1]*Sigma_bar[1][1] + Icro_2[1][2]*Sigma_bar[2][1];
+ Sigma_bar[1][2] = Icro_2[1][0]*Sigma_bar[0][2] + Icro_2[1][1]*Sigma_bar[1][2] + Icro_2[1][2]*Sigma_bar[2][2];
+ Sigma_bar[2][0] = Icro_2[2][0]*Sigma_bar[0][0] + Icro_2[2][1]*Sigma_bar[1][0] + Icro_2[2][2]*Sigma_bar[2][0];
+ Sigma_bar[2][1] = Icro_2[2][0]*Sigma_bar[0][1] + Icro_2[2][1]*Sigma_bar[1][1] + Icro_2[2][2]*Sigma_bar[2][1];
+ Sigma_bar[2][2] = Icro_2[2][0]*Sigma_bar[0][2] + Icro_2[2][1]*Sigma_bar[1][2] + Icro_2[2][2]*Sigma_bar[2][2];
+
+ up_check = 1;
+ break;
+
+ // 2 for line update only
+ case 2:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 3 for cross and line update together
+ case 3:
+ // mu_bar + K*(Z-Z_hat) = mu
+ mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
+ mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
+
+ // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
+ Sigma_bar[0][0] = Icro_2[0][0]*Sigma_bar[0][0] + Icro_2[0][1]*Sigma_bar[1][0] + Icro_2[0][2]*Sigma_bar[2][0];
+ Sigma_bar[0][1] = Icro_2[0][0]*Sigma_bar[0][1] + Icro_2[0][1]*Sigma_bar[1][1] + Icro_2[0][2]*Sigma_bar[2][1];
+ Sigma_bar[0][2] = Icro_2[0][0]*Sigma_bar[0][2] + Icro_2[0][1]*Sigma_bar[1][2] + Icro_2[0][2]*Sigma_bar[2][2];
+ Sigma_bar[1][0] = Icro_2[1][0]*Sigma_bar[0][0] + Icro_2[1][1]*Sigma_bar[1][0] + Icro_2[1][2]*Sigma_bar[2][0];
+ Sigma_bar[1][1] = Icro_2[1][0]*Sigma_bar[0][1] + Icro_2[1][1]*Sigma_bar[1][1] + Icro_2[1][2]*Sigma_bar[2][1];
+ Sigma_bar[1][2] = Icro_2[1][0]*Sigma_bar[0][2] + Icro_2[1][1]*Sigma_bar[1][2] + Icro_2[1][2]*Sigma_bar[2][2];
+ Sigma_bar[2][0] = Icro_2[2][0]*Sigma_bar[0][0] + Icro_2[2][1]*Sigma_bar[1][0] + Icro_2[2][2]*Sigma_bar[2][0];
+ Sigma_bar[2][1] = Icro_2[2][0]*Sigma_bar[0][1] + Icro_2[2][1]*Sigma_bar[1][1] + Icro_2[2][2]*Sigma_bar[2][1];
+ Sigma_bar[2][2] = Icro_2[2][0]*Sigma_bar[0][2] + Icro_2[2][1]*Sigma_bar[1][2] + Icro_2[2][2]*Sigma_bar[2][2];
+
+ up_check = 1;
+ break;
+ }
+ }
+ else if(update_state == 2)
+ {
+ switch(select_ups)
+ {
+ // 0 for no update
+ case 0:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 1 for cross update only
+ case 1:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 2 for line update only
+ case 2:
+ // mu = mu_bar + K*(Z-Z_hat)
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
+
+ // Sigma = (I - K*H) * Sigma_bar
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
+
+ up_check = 2;
+ break;
+
+ // 3 for cross and line update together
+ case 3:
+ // mu = mu_bar + K*(Z-Z_hat)
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
+
+ // Sigma = (I - K*H) * Sigma_bar
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
+
+ up_check = 2;
+ break;
+ }
+ }
+ else
+ {
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ }
+
+ up_dis = 0;
+ }
+ else if(up_dis <= -up_dis_r)
+ {
+ // update
+ if(update_state == 1)
+ {
+ switch(select_ups)
+ {
+ // 0 for no update
+ case 0:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 1 for cross update only
+ case 1:
+ // mu_bar + K*(Z-Z_hat) = mu
+ mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
+ mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
+
+ // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
+ Sigma_bar[0][0] = Icro_2[0][0]*Sigma_bar[0][0] + Icro_2[0][1]*Sigma_bar[1][0] + Icro_2[0][2]*Sigma_bar[2][0];
+ Sigma_bar[0][1] = Icro_2[0][0]*Sigma_bar[0][1] + Icro_2[0][1]*Sigma_bar[1][1] + Icro_2[0][2]*Sigma_bar[2][1];
+ Sigma_bar[0][2] = Icro_2[0][0]*Sigma_bar[0][2] + Icro_2[0][1]*Sigma_bar[1][2] + Icro_2[0][2]*Sigma_bar[2][2];
+ Sigma_bar[1][0] = Icro_2[1][0]*Sigma_bar[0][0] + Icro_2[1][1]*Sigma_bar[1][0] + Icro_2[1][2]*Sigma_bar[2][0];
+ Sigma_bar[1][1] = Icro_2[1][0]*Sigma_bar[0][1] + Icro_2[1][1]*Sigma_bar[1][1] + Icro_2[1][2]*Sigma_bar[2][1];
+ Sigma_bar[1][2] = Icro_2[1][0]*Sigma_bar[0][2] + Icro_2[1][1]*Sigma_bar[1][2] + Icro_2[1][2]*Sigma_bar[2][2];
+ Sigma_bar[2][0] = Icro_2[2][0]*Sigma_bar[0][0] + Icro_2[2][1]*Sigma_bar[1][0] + Icro_2[2][2]*Sigma_bar[2][0];
+ Sigma_bar[2][1] = Icro_2[2][0]*Sigma_bar[0][1] + Icro_2[2][1]*Sigma_bar[1][1] + Icro_2[2][2]*Sigma_bar[2][1];
+ Sigma_bar[2][2] = Icro_2[2][0]*Sigma_bar[0][2] + Icro_2[2][1]*Sigma_bar[1][2] + Icro_2[2][2]*Sigma_bar[2][2];
+
+ up_check = 1;
+ break;
+
+ // 2 for line update only
+ case 2:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 3 for cross and line update together
+ case 3:
+ // mu_bar + K*(Z-Z_hat) = mu
+ mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
+ mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
+
+ // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
+ Sigma_bar[0][0] = Icro_2[0][0]*Sigma_bar[0][0] + Icro_2[0][1]*Sigma_bar[1][0] + Icro_2[0][2]*Sigma_bar[2][0];
+ Sigma_bar[0][1] = Icro_2[0][0]*Sigma_bar[0][1] + Icro_2[0][1]*Sigma_bar[1][1] + Icro_2[0][2]*Sigma_bar[2][1];
+ Sigma_bar[0][2] = Icro_2[0][0]*Sigma_bar[0][2] + Icro_2[0][1]*Sigma_bar[1][2] + Icro_2[0][2]*Sigma_bar[2][2];
+ Sigma_bar[1][0] = Icro_2[1][0]*Sigma_bar[0][0] + Icro_2[1][1]*Sigma_bar[1][0] + Icro_2[1][2]*Sigma_bar[2][0];
+ Sigma_bar[1][1] = Icro_2[1][0]*Sigma_bar[0][1] + Icro_2[1][1]*Sigma_bar[1][1] + Icro_2[1][2]*Sigma_bar[2][1];
+ Sigma_bar[1][2] = Icro_2[1][0]*Sigma_bar[0][2] + Icro_2[1][1]*Sigma_bar[1][2] + Icro_2[1][2]*Sigma_bar[2][2];
+ Sigma_bar[2][0] = Icro_2[2][0]*Sigma_bar[0][0] + Icro_2[2][1]*Sigma_bar[1][0] + Icro_2[2][2]*Sigma_bar[2][0];
+ Sigma_bar[2][1] = Icro_2[2][0]*Sigma_bar[0][1] + Icro_2[2][1]*Sigma_bar[1][1] + Icro_2[2][2]*Sigma_bar[2][1];
+ Sigma_bar[2][2] = Icro_2[2][0]*Sigma_bar[0][2] + Icro_2[2][1]*Sigma_bar[1][2] + Icro_2[2][2]*Sigma_bar[2][2];
+
+ up_check = 1;
+ break;
+ }
+ }
+ else if(update_state == 2)
+ {
+ switch(select_ups)
+ {
+ // 0 for no update
+ case 0:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 1 for cross update only
+ case 1:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 2 for line update only
+ case 2:
+ // mu = mu_bar + K*(Z-Z_hat)
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
+
+ // Sigma = (I - K*H) * Sigma_bar
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
+
+ up_check = 2;
+ break;
+
+ // 3 for cross and line update together
+ case 3:
+ // mu = mu_bar + K*(Z-Z_hat)
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
+
+ // Sigma = (I - K*H) * Sigma_bar
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
+
+ up_check = 2;
+ break;
+ }
+ }
+ else
+ {
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ }
+
+ up_dis = 0;
+ }
+ else if(up_rot >= up_rot_r)
+ {
+ // update
+ if(update_state == 1)
+ {
+ switch(select_ups)
+ {
+ // 0 for no update
+ case 0:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 1 for cross update only
+ case 1:
+ // mu_bar + K*(Z-Z_hat) = mu
+ mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
+ mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
+
+ // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
+ Sigma_bar[0][0] = Icro_2[0][0]*Sigma_bar[0][0] + Icro_2[0][1]*Sigma_bar[1][0] + Icro_2[0][2]*Sigma_bar[2][0];
+ Sigma_bar[0][1] = Icro_2[0][0]*Sigma_bar[0][1] + Icro_2[0][1]*Sigma_bar[1][1] + Icro_2[0][2]*Sigma_bar[2][1];
+ Sigma_bar[0][2] = Icro_2[0][0]*Sigma_bar[0][2] + Icro_2[0][1]*Sigma_bar[1][2] + Icro_2[0][2]*Sigma_bar[2][2];
+ Sigma_bar[1][0] = Icro_2[1][0]*Sigma_bar[0][0] + Icro_2[1][1]*Sigma_bar[1][0] + Icro_2[1][2]*Sigma_bar[2][0];
+ Sigma_bar[1][1] = Icro_2[1][0]*Sigma_bar[0][1] + Icro_2[1][1]*Sigma_bar[1][1] + Icro_2[1][2]*Sigma_bar[2][1];
+ Sigma_bar[1][2] = Icro_2[1][0]*Sigma_bar[0][2] + Icro_2[1][1]*Sigma_bar[1][2] + Icro_2[1][2]*Sigma_bar[2][2];
+ Sigma_bar[2][0] = Icro_2[2][0]*Sigma_bar[0][0] + Icro_2[2][1]*Sigma_bar[1][0] + Icro_2[2][2]*Sigma_bar[2][0];
+ Sigma_bar[2][1] = Icro_2[2][0]*Sigma_bar[0][1] + Icro_2[2][1]*Sigma_bar[1][1] + Icro_2[2][2]*Sigma_bar[2][1];
+ Sigma_bar[2][2] = Icro_2[2][0]*Sigma_bar[0][2] + Icro_2[2][1]*Sigma_bar[1][2] + Icro_2[2][2]*Sigma_bar[2][2];
+
+ up_check = 1;
+ break;
+
+ // 2 for line update only
+ case 2:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 3 for cross and line update together
+ case 3:
+ // mu_bar + K*(Z-Z_hat) = mu
+ mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
+ mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
+
+ // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
+ Sigma_bar[0][0] = Icro_2[0][0]*Sigma_bar[0][0] + Icro_2[0][1]*Sigma_bar[1][0] + Icro_2[0][2]*Sigma_bar[2][0];
+ Sigma_bar[0][1] = Icro_2[0][0]*Sigma_bar[0][1] + Icro_2[0][1]*Sigma_bar[1][1] + Icro_2[0][2]*Sigma_bar[2][1];
+ Sigma_bar[0][2] = Icro_2[0][0]*Sigma_bar[0][2] + Icro_2[0][1]*Sigma_bar[1][2] + Icro_2[0][2]*Sigma_bar[2][2];
+ Sigma_bar[1][0] = Icro_2[1][0]*Sigma_bar[0][0] + Icro_2[1][1]*Sigma_bar[1][0] + Icro_2[1][2]*Sigma_bar[2][0];
+ Sigma_bar[1][1] = Icro_2[1][0]*Sigma_bar[0][1] + Icro_2[1][1]*Sigma_bar[1][1] + Icro_2[1][2]*Sigma_bar[2][1];
+ Sigma_bar[1][2] = Icro_2[1][0]*Sigma_bar[0][2] + Icro_2[1][1]*Sigma_bar[1][2] + Icro_2[1][2]*Sigma_bar[2][2];
+ Sigma_bar[2][0] = Icro_2[2][0]*Sigma_bar[0][0] + Icro_2[2][1]*Sigma_bar[1][0] + Icro_2[2][2]*Sigma_bar[2][0];
+ Sigma_bar[2][1] = Icro_2[2][0]*Sigma_bar[0][1] + Icro_2[2][1]*Sigma_bar[1][1] + Icro_2[2][2]*Sigma_bar[2][1];
+ Sigma_bar[2][2] = Icro_2[2][0]*Sigma_bar[0][2] + Icro_2[2][1]*Sigma_bar[1][2] + Icro_2[2][2]*Sigma_bar[2][2];
+
+ up_check = 1;
+ break;
+ }
+ }
+ else if(update_state == 2)
+ {
+ switch(select_ups)
+ {
+ // 0 for no update
+ case 0:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 1 for cross update only
+ case 1:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 2 for line update only
+ case 2:
+ // mu = mu_bar + K*(Z-Z_hat)
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
+
+ // Sigma = (I - K*H) * Sigma_bar
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
+
+ up_check = 2;
+ break;
+
+ // 3 for cross and line update together
+ case 3:
+ // mu = mu_bar + K*(Z-Z_hat)
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
+
+ // Sigma = (I - K*H) * Sigma_bar
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
+
+ up_check = 2;
+ break;
+ }
+ }
+ else
+ {
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ }
+
+ up_rot = 0;
+ }
+ else if(up_rot <= -up_rot_r)
+ {
+ // update
+ if(update_state == 1)
+ {
+ switch(select_ups)
+ {
+ // 0 for no update
+ case 0:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 1 for cross update only
+ case 1:
+ // mu_bar + K*(Z-Z_hat) = mu
+ mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
+ mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
+
+ // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
+ Sigma_bar[0][0] = Icro_2[0][0]*Sigma_bar[0][0] + Icro_2[0][1]*Sigma_bar[1][0] + Icro_2[0][2]*Sigma_bar[2][0];
+ Sigma_bar[0][1] = Icro_2[0][0]*Sigma_bar[0][1] + Icro_2[0][1]*Sigma_bar[1][1] + Icro_2[0][2]*Sigma_bar[2][1];
+ Sigma_bar[0][2] = Icro_2[0][0]*Sigma_bar[0][2] + Icro_2[0][1]*Sigma_bar[1][2] + Icro_2[0][2]*Sigma_bar[2][2];
+ Sigma_bar[1][0] = Icro_2[1][0]*Sigma_bar[0][0] + Icro_2[1][1]*Sigma_bar[1][0] + Icro_2[1][2]*Sigma_bar[2][0];
+ Sigma_bar[1][1] = Icro_2[1][0]*Sigma_bar[0][1] + Icro_2[1][1]*Sigma_bar[1][1] + Icro_2[1][2]*Sigma_bar[2][1];
+ Sigma_bar[1][2] = Icro_2[1][0]*Sigma_bar[0][2] + Icro_2[1][1]*Sigma_bar[1][2] + Icro_2[1][2]*Sigma_bar[2][2];
+ Sigma_bar[2][0] = Icro_2[2][0]*Sigma_bar[0][0] + Icro_2[2][1]*Sigma_bar[1][0] + Icro_2[2][2]*Sigma_bar[2][0];
+ Sigma_bar[2][1] = Icro_2[2][0]*Sigma_bar[0][1] + Icro_2[2][1]*Sigma_bar[1][1] + Icro_2[2][2]*Sigma_bar[2][1];
+ Sigma_bar[2][2] = Icro_2[2][0]*Sigma_bar[0][2] + Icro_2[2][1]*Sigma_bar[1][2] + Icro_2[2][2]*Sigma_bar[2][2];
+
+ up_check = 1;
+ break;
+
+ // 2 for line update only
+ case 2:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 3 for cross and line update together
+ case 3:
+ // mu_bar + K*(Z-Z_hat) = mu
+ mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
+ mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
+
+ // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
+ Sigma_bar[0][0] = Icro_2[0][0]*Sigma_bar[0][0] + Icro_2[0][1]*Sigma_bar[1][0] + Icro_2[0][2]*Sigma_bar[2][0];
+ Sigma_bar[0][1] = Icro_2[0][0]*Sigma_bar[0][1] + Icro_2[0][1]*Sigma_bar[1][1] + Icro_2[0][2]*Sigma_bar[2][1];
+ Sigma_bar[0][2] = Icro_2[0][0]*Sigma_bar[0][2] + Icro_2[0][1]*Sigma_bar[1][2] + Icro_2[0][2]*Sigma_bar[2][2];
+ Sigma_bar[1][0] = Icro_2[1][0]*Sigma_bar[0][0] + Icro_2[1][1]*Sigma_bar[1][0] + Icro_2[1][2]*Sigma_bar[2][0];
+ Sigma_bar[1][1] = Icro_2[1][0]*Sigma_bar[0][1] + Icro_2[1][1]*Sigma_bar[1][1] + Icro_2[1][2]*Sigma_bar[2][1];
+ Sigma_bar[1][2] = Icro_2[1][0]*Sigma_bar[0][2] + Icro_2[1][1]*Sigma_bar[1][2] + Icro_2[1][2]*Sigma_bar[2][2];
+ Sigma_bar[2][0] = Icro_2[2][0]*Sigma_bar[0][0] + Icro_2[2][1]*Sigma_bar[1][0] + Icro_2[2][2]*Sigma_bar[2][0];
+ Sigma_bar[2][1] = Icro_2[2][0]*Sigma_bar[0][1] + Icro_2[2][1]*Sigma_bar[1][1] + Icro_2[2][2]*Sigma_bar[2][1];
+ Sigma_bar[2][2] = Icro_2[2][0]*Sigma_bar[0][2] + Icro_2[2][1]*Sigma_bar[1][2] + Icro_2[2][2]*Sigma_bar[2][2];
+
+ up_check = 1;
+ break;
+ }
+ }
+ else if(update_state == 2)
+ {
+ switch(select_ups)
+ {
+ // 0 for no update
+ case 0:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 1 for cross update only
+ case 1:
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ break;
+
+ // 2 for line update only
+ case 2:
+ // mu = mu_bar + K*(Z-Z_hat)
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
+
+ // Sigma = (I - K*H) * Sigma_bar
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
+
+ up_check = 2;
+ break;
+
+ // 3 for cross and line update together
+ case 3:
+ // mu = mu_bar + K*(Z-Z_hat)
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
+
+ // Sigma = (I - K*H) * Sigma_bar
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
+
+ up_check = 2;
+ break;
+ }
+ }
+ else
+ {
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ }
+
+ up_rot = 0;
+ }
+ else
+ {
+ mu_bar[0][0] = mu_bar[0][0];
+ mu_bar[1][0] = mu_bar[1][0];
+ mu_bar[2][0] = mu_bar[2][0];
+
+ Sigma_bar[0][0] = Sigma_bar[0][0];
+ Sigma_bar[0][1] = Sigma_bar[0][1];
+ Sigma_bar[0][2] = Sigma_bar[0][2];
+ Sigma_bar[1][0] = Sigma_bar[1][0];
+ Sigma_bar[1][1] = Sigma_bar[1][1];
+ Sigma_bar[1][2] = Sigma_bar[1][2];
+ Sigma_bar[2][0] = Sigma_bar[2][0];
+ Sigma_bar[2][1] = Sigma_bar[2][1];
+ Sigma_bar[2][2] = Sigma_bar[2][2];
+
+ up_check = 0;
+ }
+
+ mu[0][0] = mu_bar[0][0];
+ mu[1][0] = mu_bar[1][0];
+ mu[2][0] = mu_bar[2][0];
+
+ Sigma[0][0] = Sigma_bar[0][0];
+ Sigma[0][1] = Sigma_bar[0][1];
+ Sigma[0][2] = Sigma_bar[0][2];
+ Sigma[1][0] = Sigma_bar[1][0];
+ Sigma[1][1] = Sigma_bar[1][1];
+ Sigma[1][2] = Sigma_bar[1][2];
+ Sigma[2][0] = Sigma_bar[2][0];
+ Sigma[2][1] = Sigma_bar[2][1];
+ Sigma[2][2] = Sigma_bar[2][2];
+ }
+ // if(fusion_flag == 1) end
+ }
+
+////////// timer 5 : extended kalman filter ////////////////////////////////////////////////////////////////////////////////////
+
+ // timer 6 : navigation
+ if((t.read_us() - lastMilli6) >= LOOPTIME6) // 100000 us = 0.1 s
+ {
+ lastMilli6 = t.read_us();
+
+ // range 0~359 deg
+ mu_th_deg = mu[2][0] * 180/PI;
+
+ if(mu_th_deg < 0)
+ mu_th_deg = mu_th_deg % 360 + 360;
+ else
+ mu_th_deg = mu_th_deg % 360;
+
+ // range -179~180 deg
+ if(mu_th_deg > 180)
+ mu_th_deg_180 = mu_th_deg - 360;
+ else
+ mu_th_deg_180 = mu_th_deg;
+
+ mu_th_PI = (float)mu_th_deg_180 * PI/180;
+
+ int turn_point = 33;
+ int end_point = turn_point + 22;
+
+ if(nav_state == 1)
+ {
+ Pxd = turn_point*1*block_length;
+ Pyd = 0*1*block_length;
+ Od = 0;
+ }
+ else if(nav_state == 2)
+ {
+ Pxd = turn_point*1*block_length;
+ Pyd = 0*1*block_length;
+ Od = PI/2;
+ }
+ else if(nav_state == 3)
+ {
+ Pxd = turn_point*1*block_length;
+ Pyd = (1)*(end_point - turn_point)*1*block_length;
+ Od = PI/2;
+ }
+ else if(nav_state == 4)
+ {
+ Pxd = turn_point*1*block_length;
+ Pyd = (1)*(end_point - turn_point)*1*block_length;
+ Od = PI;
+ }
+
+ delta_Pxd = mu[0][0] - Pxd;
+ delta_Pyd = mu[1][0] - Pyd;
+ delta_Od = mu_th_PI - Od;
+
+ distance_error_P = sqrt(pow(delta_Pxd,2) + pow(delta_Pyd,2));
+ error_O = delta_Od;
+
+//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!//
+
+ if((Theta_c >= 70 && Theta_c <= 89) && (Theta_c >= -89 && Theta_c <= -70) && nav_flag == 1 && update_source == 2 && update_source == 1)
+ {
+ line_tracker = 1;
+ }
+ else
+ {
+ line_tracker = 0;
+ }
+
+ if(nav_state == 2)
+ {
+ line_tracker = 0;
+ }
+
+ if(line_tracker == 1)
+ {
+ float K_line_tracker = 0.02;
+
+ if(Theta_c >= 60) {gain_w = -K_line_tracker*(Theta_c - 90);}
+ else if(Theta_c <= -60) {gain_w = -K_line_tracker*(90 + Theta_c);}
+
+ if(distance_error_P >= 0.39)
+ {
+ gain_v = 0.15;
+ }
+ else
+ {
+ gain_v = -Kv*(delta_Pxd*cos(mu_th_PI) + delta_Pyd*sin(mu_th_PI));
+ }
+
+ vel_msg.linear.x = gain_v;
+ vel_msg.angular.z = gain_w;
+ p.publish(&vel_msg);
+
+ gain_v = -Kv*(delta_Pxd*cos(mu_th_PI) + delta_Pyd*sin(mu_th_PI));
+ gain_w = -Kw*delta_Od;
+
+ if(gain_v >= 0)
+ error_P = gain_v;
+ else
+ error_P = -gain_v;
+
+ if(gain_w >= 0)
+ error_O = gain_w;
+ else
+ error_O = -gain_w;
+ }
+ else
+ {
+ if(distance_error_P >= 0.39)
+ {
+ gain_v = 0.15;
+ gain_w = -Kw*delta_Od;
+ }
+ else
+ {
+ gain_v = -Kv*(delta_Pxd*cos(mu_th_PI) + delta_Pyd*sin(mu_th_PI));
+ gain_w = -Kw*delta_Od;
+ }
+
+ if(gain_v >= 0)
+ error_P = gain_v;
+ else
+ error_P = -gain_v;
+
+ if(gain_w >= 0)
+ error_O = gain_w;
+ else
+ error_O = -gain_w;
+
+ if(nav_flag == 1)
+ {
+ vel_msg.linear.x = gain_v;
+ vel_msg.angular.z = gain_w;
+ p.publish(&vel_msg);
+ }
+
+ }
+
+ if(error_P < 0.01 && error_O < PI/90)
+ {
+ vel_msg.linear.x = 0;
+ vel_msg.angular.z = 0;
+ p.publish(&vel_msg);
+
+ nav_state++;
+
+ if(nav_state >= 5)
+ {
+ nav_flag = 0;
+ nav_state = 0;
+ }
+ }
+ }
+
+//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!//
+
+ // magnetometer correction
+ if(cal_flag == 1)
+ {
+ magx_max = magx[0];
+ magx_min = magx[0];
+ magy_max = magy[0];
+ magy_min = magy[0];
+
+ magx_max2 = magx2[0];
+ magx_min2 = magx2[0];
+ magy_max2 = magy2[0];
+ magy_min2 = magy2[0];
+
+ for(int i = 1 ; i < ind ; i++)
+ {
+ if(magx[i] > magx_max) // find x max
+ magx_max = magx[i];
+ if(magx[i] < magx_min) // find x min
+ magx_min = magx[i];
+ if(magy[i] > magy_max) // find y max
+ magy_max = magy[i];
+ if(magy[i] < magy_min) // find y min
+ magy_min = magy[i];
+
+ if(magx2[i] > magx_max2) // find x max
+ magx_max2 = magx2[i];
+ if(magx2[i] < magx_min2) // find x min
+ magx_min2 = magx2[i];
+ if(magy2[i] > magy_max2) // find y max
+ magy_max2 = magy2[i];
+ if(magy2[i] < magy_min2) // find y min
+ magy_min2 = magy2[i];
+ }
+
+ magx0 = (magx_max + magx_min) / 2; // center x
+ magy0 = (magy_max + magy_min) / 2; // center y
+
+ magx02 = (magx_max2 + magx_min2) / 2; // center x
+ magy02 = (magy_max2 + magy_min2) / 2; // center y
+
+ Magn_axis_zero[0] = magx0;
+ Magn_axis_zero[1] = magy0;
+
+ Magn_axis_zero2[0] = magx02;
+ Magn_axis_zero2[1] = magy02;
+
+ a = (magx_max - magx_min) / 2; // long axis
+ b = (magy_max - magy_min) / 2; // short axis
+
+ a2 = (magx_max2 - magx_min2) / 2; // long axis
+ b2 = (magy_max2 - magy_min2) / 2; // short axis
+
+ Bex = (Magn_axis_data_f[0] - Magn_axis_zero[0]) / a;
+ Bey = (Magn_axis_data_f[1] - Magn_axis_zero[1]) / b;
+
+ Bex2 = (Magn_axis_data_f2[0] - Magn_axis_zero2[0]) / a2;
+ Bey2 = (Magn_axis_data_f2[1] - Magn_axis_zero2[1]) / b2;
+
+ Be = (sqrt(Bex*Bex + Bey*Bey) + sqrt(Bex2*Bex2 + Bey2*Bey2)) / 2;
+
+ cal_flag = 0; // calculate finish
+ th = 0;
+ ind = 0;
+ ind_ = 0;
+ BK_ = 1;
+ }
+ } // while(1) end
+} // int main() end
+
+// Kalman filter and observer //////////////////////////////////////////////////////////////////////////////////////////////
+
+void read_sensor(void)
+{
+ // read sensor raw data from LSM9DS0_SH.h
+ read_IMU();
+
+ // sensor filter data
+ // gyro z
+ Gyro_axis_data_f[2] = lpf(Wz,Gyro_axis_data_f_old[2],10);
+ Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
+
+ // magn x , magn y , magn z
+ Magn_axis_data_f[0] = lpf(Mx,Magn_axis_data_f_old[0],12);
+ Magn_axis_data_f_old[0] = Magn_axis_data_f[0];
+ Magn_axis_data_f[1] = lpf(My,Magn_axis_data_f_old[1],12);
+ Magn_axis_data_f_old[1] = Magn_axis_data_f[1];
+
+ Magn_axis_data_f2[0] = lpf(M2x,Magn_axis_data_f_old2[0],12);
+ Magn_axis_data_f_old2[0] = Magn_axis_data_f2[0];
+ Magn_axis_data_f2[1] = lpf(M2y,Magn_axis_data_f_old2[1],12);
+ Magn_axis_data_f_old2[1] = Magn_axis_data_f2[1];
+
+ // sensor after correction
+ // gyro z
+ Gyro_scale[2] = (Gyro_axis_data_f[2] - Gyro_axis_zero[2]);
+ w_s = Gyro_scale[2];
+
+ // magn x , magn y
+ Magn_scale[0] = (Magn_axis_data_f[0] - Magn_axis_zero[0]) / a;
+ Magn_scale[1] = (Magn_axis_data_f[1] - Magn_axis_zero[1]) / b;
+
+ Magn_scale2[0] = (Magn_axis_data_f2[0] - Magn_axis_zero2[0]) / a2;
+ Magn_scale2[1] = (Magn_axis_data_f2[1] - Magn_axis_zero2[1]) / b2;
+
+ // trust index
+ Q = (Magn_scale[0]*Magn_scale2[0] + Magn_scale[1]*Magn_scale2[1]) / (Be * Be);
+ Q = Q - 1;
+
+ // magnetometer angle
+ if(Magn_scale[0]>0 && Magn_scale[1]>0)
+ phi = 2*PI - atan((Magn_scale[1])/(Magn_scale[0]));
+ else if(Magn_scale[0]<0 && Magn_scale[1]>0)
+ phi = 2*PI - (PI - atan(-(Magn_scale[1])/(Magn_scale[0])));
+ else if(Magn_scale[0]<0 && Magn_scale[1]<0)
+ phi = 2*PI - (PI + atan((Magn_scale[1])/(Magn_scale[0])));
+ else if(Magn_scale[0]>0 && Magn_scale[1]<0)
+ phi = 2*PI - (2*PI - atan(-(Magn_scale[1])/(Magn_scale[0])));
+
+ if(Magn_scale2[0]>0 && Magn_scale2[1]>0)
+ phi2 = 2*PI - atan((Magn_scale2[1])/(Magn_scale2[0]));
+ else if(Magn_scale2[0]<0 && Magn_scale2[1]>0)
+ phi2 = 2*PI - (PI - atan(-(Magn_scale2[1])/(Magn_scale2[0])));
+ else if(Magn_scale2[0]<0 && Magn_scale2[1]<0)
+ phi2 = 2*PI - (PI + atan((Magn_scale2[1])/(Magn_scale2[0])));
+ else if(Magn_scale2[0]>0 && Magn_scale2[1]<0)
+ phi2 = 2*PI - (2*PI - atan(-(Magn_scale2[1])/(Magn_scale2[0])));
+
+ // gyro bias average
+ if(gyro_flag == 0)
+ {
+ gyro_count++;
+
+ if(gyro_count == 150)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 155)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 160)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 165)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 170)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 175)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 180)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 185)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count == 190)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ index_count++;
+ }
+ else if(gyro_count >= 195)
+ {
+ gyro_init[index_count] = Gyro_axis_data_f[2];
+ int z = 0;
+ for(z=0 ; z<10 ; z++)
+ gyro_sum = gyro_sum + gyro_init[z];
+
+ Gyro_axis_zero[2] = gyro_sum / 10;
+ index_count = 0;
+ gyro_count = 0;
+ gyro_flag = 1;
+ }
+ }
+
+ if(gyro_flag == 1)
+ {
+ // w_s deadzone
+ int flag_d_1 = 0;
+ int flag_d_2 = 0;
+ flag_d_1 = w_s <= 0.006;
+ flag_d_2 = w_s >= -0.006;
+
+ if((flag_d_1 + flag_d_2) == 2)
+ w_s_d = 0;
+ else
+ w_s_d = w_s;
+
+ // w_s_d integral
+ theta_s = theta_s + T*w_s_d;
+
+ // theta_s_deg range 0~359 deg
+ theta_s_deg = theta_s * 180/PI;
+
+ if(theta_s_deg < 0)
+ theta_s_deg = theta_s_deg % 360 + 360;
+ else
+ theta_s_deg = theta_s_deg % 360;
+ }
+
+ // phi initialize
+ if(phi_init_flag == 1)
+ {
+ phi_init_count++;
+
+ if(phi_init_count >= 200)
+ {
+ // take current phi data for reseting absolute orientation to zero
+ phi_init = phi;
+ phi_init2 = phi2;
+
+ // reset KF and observer data
+ phi_m_a_deg = 0;
+ phi_m2_a_deg = 0;
+ jump_count = 0;
+ jump_count2 = 0;
+ phi_m_deg_1 = 0;
+ phi_m2_deg_1 = 0;
+ phi_m_deg_2 = 0;
+ phi_m2_deg_2 = 0;
+ phi_m_a_avg = 0;
+ theta_s = 0;
+ theta_me = 0;
+ theta_fu = 0;
+ theta_fu_old = 0;
+ bias = 0;
+ bias_old = 0;
+ y = 0;
+
+ // encoder odometry reset
+ x = 0;
+ y_ = 0;
+ th = 0;
+ x_enc = 0;
+ y_enc = 0;
+ th_enc = 0;
+ x_emg = 0;
+ y_emg = 0;
+ th_emg = 0;
+
+ // reset EKF data
+ for(int i = 0 ; i < 3 ; i++)
+ {
+ mu_bar[i][0] = 0;
+ mu[i][0] = 0;
+
+ for(int j = 0 ; j < 3 ; j++)
+ {
+ Sigma_bar[i][j] = 0;
+ Sigma[i][j] = 0;
+ }
+ }
+
+ // stop initialization
+ phi_init_count = 0;
+ phi_init_flag = 0;
+
+ // start fusion
+ fusion_flag = 1;
+ B_ = 1; // dark
+ }
+ }
+
+ // phi_m_deg range 0~359 deg
+ phi_m = phi - phi_init;
+ phi_m_deg = phi_m * 180/PI;
+
+ if(phi_m_deg < 0)
+ phi_m_deg = phi_m_deg % 360 + 360;
+ else
+ phi_m_deg = phi_m_deg % 360;
+
+ phi_m2 = phi2 - phi_init2;
+ phi_m2_deg = phi_m2 * 180/PI;
+
+ if(phi_m2_deg < 0)
+ phi_m2_deg = phi_m2_deg % 360 + 360;
+ else
+ phi_m2_deg = phi_m2_deg % 360;
+
+ // phi_m_a accumulate form
+ diff_phi_count++;
+
+ if(diff_phi_flag == 0)
+ {
+ phi_m_deg_1 = phi_m_deg;
+ phi_m2_deg_1 = phi_m2_deg;
+ diff_phi_flag = 1;
+ }
+ else if(diff_phi_flag == 1)
+ {
+ if(diff_phi_count >= 2)
+ {
+ phi_m_deg_2 = phi_m_deg + 360*jump_count;
+ phi_m_deg_T = phi_m_deg_2 - phi_m_deg_1;
+
+ if(phi_m_deg_T <= -300)
+ {
+ phi_m_a_deg = phi_m_deg_2 + 360;
+ phi_m_deg_1 = phi_m_a_deg;
+ jump_count++;
+ }
+ else if(phi_m_deg_T >= 300)
+ {
+ phi_m_a_deg = phi_m_deg_2 - 360;
+ phi_m_deg_1 = phi_m_a_deg;
+ jump_count--;
+ }
+ else
+ {
+ phi_m_a_deg = phi_m_deg_2;
+ phi_m_deg_1 = phi_m_a_deg;
+ }
+
+ phi_m_a = phi_m_a_deg * PI/180;
+
+ ////////////////////////////////////////////////////////////////////
+
+ phi_m2_deg_2 = phi_m2_deg + 360*jump_count2;
+ phi_m2_deg_T = phi_m2_deg_2 - phi_m2_deg_1;
+
+ if(phi_m2_deg_T <= -300)
+ {
+ phi_m2_a_deg = phi_m2_deg_2 + 360;
+ phi_m2_deg_1 = phi_m2_a_deg;
+ jump_count2++;
+ }
+ else if(phi_m2_deg_T >= 300)
+ {
+ phi_m2_a_deg = phi_m2_deg_2 - 360;
+ phi_m2_deg_1 = phi_m2_a_deg;
+ jump_count2--;
+ }
+ else
+ {
+ phi_m2_a_deg = phi_m2_deg_2;
+ phi_m2_deg_1 = phi_m2_a_deg;
+ }
+
+ phi_m2_a = phi_m2_a_deg * PI/180;
+
+ phi_m_a_avg = 0.7*phi_m_a + 0.3*phi_m2_a;
+
+ diff_phi_count = 0;
+ }
+ }
+
+ // theta_me Kalman filter
+ // prediction
+ theta_me_bar = At*theta_me + Bt*omega_z;
+ sigma_bar = At*sigma*At + Rt;
+
+ if(Q >= -0.009 && Q <= 0.009)
+ {
+ if(Q >= 0)
+ V = 0.74*Q;
+ else
+ V = -1.13*Q;
+
+ update_flag = 1;
+ G_ = 0;
+ }
+ else
+ {
+ update_flag = 0;
+ G_ = 1;
+ }
+
+ // update distance
+ update_distance = update_distance + T*vel_x;
+ update_rotation = update_rotation + T*omega_z;
+
+ if(update_distance >= 0.1)
+ {
+ // update
+ if(update_flag == 1)
+ {
+ K = sigma_bar*Ct / (Ct*sigma_bar*Ct + V);
+ theta_me = theta_me_bar + K*(phi_m_a_avg - Ct*theta_me_bar);
+ sigma = (1 - K*Ct)*sigma_bar;
+ }
+ else
+ {
+ theta_me = theta_me_bar;
+ sigma = sigma_bar;
+ }
+
+ update_distance = 0;
+ }
+ else if(update_distance <= -0.1)
+ {
+ // update
+ if(update_flag == 1)
+ {
+ K = sigma_bar*Ct / (Ct*sigma_bar*Ct + V);
+ theta_me = theta_me_bar + K*(phi_m_a_avg - Ct*theta_me_bar);
+ sigma = (1 - K*Ct)*sigma_bar;
+ }
+ else
+ {
+ theta_me = theta_me_bar;
+ sigma = sigma_bar;
+ }
+
+ update_distance = 0;
+ }
+ else if(update_rotation >= PI/8)
+ {
+ // update
+ if(update_flag == 1)
+ {
+ K = sigma_bar*Ct / (Ct*sigma_bar*Ct + V);
+ theta_me = theta_me_bar + K*(phi_m_a_avg - Ct*theta_me_bar);
+ sigma = (1 - K*Ct)*sigma_bar;
+ }
+ else
+ {
+ theta_me = theta_me_bar;
+ sigma = sigma_bar;
+ }
+
+ update_rotation = 0;
+ }
+ else if(update_rotation <= -PI/8)
+ {
+ // update
+ if(update_flag == 1)
+ {
+ K = sigma_bar*Ct / (Ct*sigma_bar*Ct + V);
+ theta_me = theta_me_bar + K*(phi_m_a_avg - Ct*theta_me_bar);
+ sigma = (1 - K*Ct)*sigma_bar;
+ }
+ else
+ {
+ theta_me = theta_me_bar;
+ sigma = sigma_bar;
+ }
+
+ update_rotation = 0;
+ }
+ else
+ {
+ theta_me = theta_me_bar;
+ sigma = sigma_bar;
+ }
+
+ // theta_me_deg range 0~359 deg
+ theta_me_deg = theta_me * 180/PI;
+
+ if(theta_me_deg < 0)
+ theta_me_deg = theta_me_deg % 360 + 360;
+ else
+ theta_me_deg = theta_me_deg % 360;
+
+ // theta_e_deg range 0~359 deg
+ th_deg = th * 180/PI;
+
+ if(th_deg < 0)
+ th_deg = th_deg % 360 + 360;
+ else
+ th_deg = th_deg % 360;
+
+ if(fusion_flag == 1)
+ {
+ // Kalman filter
+ L1 = 7;
+ L2 = 13;
+
+ y = theta_me;
+
+ if(bias_flag == 1)
+ bias_add = bias_add - r_01*r_01*0.000001 + r_01*0.000000000001;
+ else
+ bias_add = 0;
+
+ // sensor fusion algorithm
+ theta_fu = (theta_fu_old + T*(w_s_d + bias_add) + L1*T*y + T*bias) / (1 + L1*T);
+ theta_fu_old = theta_fu;
+
+ bias = bias_old + L2*T*(y - theta_fu);
+ bias_old = bias;
+
+ // theta_fu_deg range 0~359 deg
+ theta_fu_deg = theta_fu * 180/PI;
+
+ if(theta_fu_deg < 0)
+ theta_fu_deg = theta_fu_deg % 360 + 360;
+ else
+ theta_fu_deg = theta_fu_deg % 360;
+
+ // theta_fu_deg_180 range -179~180 deg
+ if(theta_fu_deg > 180)
+ theta_fu_deg_180 = theta_fu_deg - 360;
+ else
+ theta_fu_deg_180 = theta_fu_deg;
+
+ theta_fu_PI = (float)theta_fu_deg_180 * PI/180;
+ } // if(fusion_flag == 1) end
+}
+
+// Kalman filter and observer //////////////////////////////////////////////////////////////////////////////////////////////
+
+float lpf(float input, float output_old, float frequency)
+{
+ float output = 0;
+
+ output = (output_old + frequency*T*input) / (1 + frequency*T);
+
+ return output;
+}