lib
Dependencies: mbed ros_lib_indigo
main.cpp
- Committer:
- tea5062001
- Date:
- 2017-10-02
- Revision:
- 0:c94b2f0a4409
File content as of revision 0:c94b2f0a4409:
#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; }