LDSC_Robotics_TAs
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;
}