LDSC_Robotics_TAs / Mbed 2 deprecated car20170828

lib

Dependencies:   mbed ros_lib_indigo

main.cpp@0:c94b2f0a4409, 2017-10-02 (annotated)

Committer:
tea5062001
Date:
Mon Oct 02 11:30:47 2017 +0000
Revision:
0:c94b2f0a4409
lib

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tea5062001 0:c94b2f0a4409 1 #include "mbed.h"
tea5062001 0:c94b2f0a4409 2 #include "led.h"
tea5062001 0:c94b2f0a4409 3 #include "uart_send.h"
tea5062001 0:c94b2f0a4409 4 #include <ros.h>
tea5062001 0:c94b2f0a4409 5 #include <geometry_msgs/Vector3.h>
tea5062001 0:c94b2f0a4409 6 #include <geometry_msgs/Twist.h>
tea5062001 0:c94b2f0a4409 7 #include "LSM9DS0_SH.h"
tea5062001 0:c94b2f0a4409 8 #include <math.h>
tea5062001 0:c94b2f0a4409 9
tea5062001 0:c94b2f0a4409 10 #define r_01 ((double)rand()/(RAND_MAX)) // uniform distribution 0 ~ 1
tea5062001 0:c94b2f0a4409 11 #define constrain(amt,low,high) ((amt)<(low) ? (low) : ((amt) > (high) ? (high) : (amt)))
tea5062001 0:c94b2f0a4409 12
tea5062001 0:c94b2f0a4409 13 // timer 1 : sensor fusion
tea5062001 0:c94b2f0a4409 14 #define LOOPTIME 2000 // 0.002 s
tea5062001 0:c94b2f0a4409 15 unsigned long lastMilli = 0;
tea5062001 0:c94b2f0a4409 16 // sampling time
tea5062001 0:c94b2f0a4409 17 float T = 0.002;
tea5062001 0:c94b2f0a4409 18
tea5062001 0:c94b2f0a4409 19 // timer 2 : publish cmd_vel for mag_cor
tea5062001 0:c94b2f0a4409 20 #define LOOPTIME2 100000 // 0.1 s
tea5062001 0:c94b2f0a4409 21 unsigned long lastMilli2 = 0;
tea5062001 0:c94b2f0a4409 22
tea5062001 0:c94b2f0a4409 23 // timer 3 : take part of mag data
tea5062001 0:c94b2f0a4409 24 #define LOOPTIME3 300000 // 0.3 s
tea5062001 0:c94b2f0a4409 25 unsigned long lastMilli3 = 0;
tea5062001 0:c94b2f0a4409 26
tea5062001 0:c94b2f0a4409 27 // timer 5 : extended kalman filter
tea5062001 0:c94b2f0a4409 28 #define LOOPTIME5 20000 // 0.02 s
tea5062001 0:c94b2f0a4409 29 unsigned long lastMilli5 = 0;
tea5062001 0:c94b2f0a4409 30 // sampling time
tea5062001 0:c94b2f0a4409 31 float T2 = 0.02;
tea5062001 0:c94b2f0a4409 32
tea5062001 0:c94b2f0a4409 33 // timer 6 : navigation
tea5062001 0:c94b2f0a4409 34 #define LOOPTIME6 100000 // 0.1 s
tea5062001 0:c94b2f0a4409 35 unsigned long lastMilli6 = 0;
tea5062001 0:c94b2f0a4409 36
tea5062001 0:c94b2f0a4409 37 Timer t;
tea5062001 0:c94b2f0a4409 38
tea5062001 0:c94b2f0a4409 39 ros::NodeHandle nh;
tea5062001 0:c94b2f0a4409 40
tea5062001 0:c94b2f0a4409 41 geometry_msgs::Twist vel_msg;
tea5062001 0:c94b2f0a4409 42 ros::Publisher p("andbot/cmd_vel", &vel_msg);
tea5062001 0:c94b2f0a4409 43 unsigned long current_time = 0;
tea5062001 0:c94b2f0a4409 44 unsigned long last_time = 0;
tea5062001 0:c94b2f0a4409 45
tea5062001 0:c94b2f0a4409 46 // car information
tea5062001 0:c94b2f0a4409 47 double wheelRadius = 0.078;
tea5062001 0:c94b2f0a4409 48 double wheelSeparation = 0.393;
tea5062001 0:c94b2f0a4409 49 double omega_right = 0.0;
tea5062001 0:c94b2f0a4409 50 double omega_left = 0.0;
tea5062001 0:c94b2f0a4409 51 double omega_z = 0.0;
tea5062001 0:c94b2f0a4409 52 double vel_x = 0.0;
tea5062001 0:c94b2f0a4409 53 double th = 0.0;
tea5062001 0:c94b2f0a4409 54 int th_deg = 0;
tea5062001 0:c94b2f0a4409 55 double x = 0.0;
tea5062001 0:c94b2f0a4409 56 double y_ = 0.0;
tea5062001 0:c94b2f0a4409 57
tea5062001 0:c94b2f0a4409 58 // start car rotation for magnetometer correction flag
tea5062001 0:c94b2f0a4409 59 int check_flag = 0;
tea5062001 0:c94b2f0a4409 60
tea5062001 0:c94b2f0a4409 61 // magnetometer correction data
tea5062001 0:c94b2f0a4409 62 // magnetometer 1
tea5062001 0:c94b2f0a4409 63 float magx[100] = {0};
tea5062001 0:c94b2f0a4409 64 float magy[100] = {0};
tea5062001 0:c94b2f0a4409 65 int ind = 0;
tea5062001 0:c94b2f0a4409 66 int ind_ = 0;
tea5062001 0:c94b2f0a4409 67 float cal_flag = 0;
tea5062001 0:c94b2f0a4409 68 float magx_max = 0;
tea5062001 0:c94b2f0a4409 69 float magx_min = 0;
tea5062001 0:c94b2f0a4409 70 float magx0 = 0;
tea5062001 0:c94b2f0a4409 71 float magy_max = 0;
tea5062001 0:c94b2f0a4409 72 float magy_min = 0;
tea5062001 0:c94b2f0a4409 73 float magy0 = 0;
tea5062001 0:c94b2f0a4409 74 float a = 0.3324; // long axis
tea5062001 0:c94b2f0a4409 75 float b = 0.3082; // short axis
tea5062001 0:c94b2f0a4409 76
tea5062001 0:c94b2f0a4409 77 // magnetometer 2
tea5062001 0:c94b2f0a4409 78 float magx2[100] = {0};
tea5062001 0:c94b2f0a4409 79 float magy2[100] = {0};
tea5062001 0:c94b2f0a4409 80 float magx_max2 = 0;
tea5062001 0:c94b2f0a4409 81 float magx_min2 = 0;
tea5062001 0:c94b2f0a4409 82 float magx02 = 0;
tea5062001 0:c94b2f0a4409 83 float magy_max2 = 0;
tea5062001 0:c94b2f0a4409 84 float magy_min2 = 0;
tea5062001 0:c94b2f0a4409 85 float magy02 = 0;
tea5062001 0:c94b2f0a4409 86 float a2 = 0.3366; // long axis
tea5062001 0:c94b2f0a4409 87 float b2 = 0.3312; // short axis
tea5062001 0:c94b2f0a4409 88
tea5062001 0:c94b2f0a4409 89 // vision data
tea5062001 0:c94b2f0a4409 90 int px = 0;
tea5062001 0:c94b2f0a4409 91 int py = 0;
tea5062001 0:c94b2f0a4409 92 int vision_flag = 0;
tea5062001 0:c94b2f0a4409 93 // calibration parameter
tea5062001 0:c94b2f0a4409 94 float alpha_u = 179.869823;
tea5062001 0:c94b2f0a4409 95 float alpha_v = 179.984858;
tea5062001 0:c94b2f0a4409 96 float alpha_s = 0;
tea5062001 0:c94b2f0a4409 97 float cx = 81.709410;
tea5062001 0:c94b2f0a4409 98 float cy = 64.218079;
tea5062001 0:c94b2f0a4409 99 // c : camera coordinate
tea5062001 0:c94b2f0a4409 100 float Zc = 420; // unit : mm
tea5062001 0:c94b2f0a4409 101 float Xc = 0;
tea5062001 0:c94b2f0a4409 102 float Yc = 0;
tea5062001 0:c94b2f0a4409 103 float Theta_c = 0;
tea5062001 0:c94b2f0a4409 104 // r : robot coordinate
tea5062001 0:c94b2f0a4409 105 float Xr = 0;
tea5062001 0:c94b2f0a4409 106 float Yr = 0;
tea5062001 0:c94b2f0a4409 107 // from odometry and cross detection to predict map coordinate
tea5062001 0:c94b2f0a4409 108 float Xp = 0;
tea5062001 0:c94b2f0a4409 109 float Yp = 0;
tea5062001 0:c94b2f0a4409 110
tea5062001 0:c94b2f0a4409 111 // subscribe feedback_wheel_angularVel
tea5062001 0:c94b2f0a4409 112 void messageCb(const geometry_msgs::Vector3& msg)
tea5062001 0:c94b2f0a4409 113 {
tea5062001 0:c94b2f0a4409 114 Y_ = !Y_;
tea5062001 0:c94b2f0a4409 115
tea5062001 0:c94b2f0a4409 116 omega_left = msg.x;
tea5062001 0:c94b2f0a4409 117 omega_right = msg.y;
tea5062001 0:c94b2f0a4409 118
tea5062001 0:c94b2f0a4409 119 omega_z = wheelRadius * (omega_right - omega_left) / wheelSeparation;
tea5062001 0:c94b2f0a4409 120 vel_x = wheelRadius * (omega_right + omega_left) / 2;
tea5062001 0:c94b2f0a4409 121
tea5062001 0:c94b2f0a4409 122 current_time = t.read_ms();
tea5062001 0:c94b2f0a4409 123 double dt = (current_time - last_time) * 0.001; // from ms to s
tea5062001 0:c94b2f0a4409 124 last_time = current_time;
tea5062001 0:c94b2f0a4409 125
tea5062001 0:c94b2f0a4409 126 double delta_th = omega_z * dt;
tea5062001 0:c94b2f0a4409 127 th += delta_th;
tea5062001 0:c94b2f0a4409 128
tea5062001 0:c94b2f0a4409 129 double delta_x = vel_x * cos(th) * dt;
tea5062001 0:c94b2f0a4409 130 x += delta_x;
tea5062001 0:c94b2f0a4409 131
tea5062001 0:c94b2f0a4409 132 double delta_y = vel_x * sin(th) * dt;
tea5062001 0:c94b2f0a4409 133 y_ += delta_y;
tea5062001 0:c94b2f0a4409 134
tea5062001 0:c94b2f0a4409 135 // for mag_cor
tea5062001 0:c94b2f0a4409 136 if(check_flag == 1)
tea5062001 0:c94b2f0a4409 137 {
tea5062001 0:c94b2f0a4409 138 if(th >= 2*PI)
tea5062001 0:c94b2f0a4409 139 {
tea5062001 0:c94b2f0a4409 140 check_flag = 0;
tea5062001 0:c94b2f0a4409 141 cal_flag = 1;
tea5062001 0:c94b2f0a4409 142 vel_msg.angular.z = 0.0;
tea5062001 0:c94b2f0a4409 143 p.publish(&vel_msg);
tea5062001 0:c94b2f0a4409 144 th = 0;
tea5062001 0:c94b2f0a4409 145 }
tea5062001 0:c94b2f0a4409 146 }
tea5062001 0:c94b2f0a4409 147 }
tea5062001 0:c94b2f0a4409 148
tea5062001 0:c94b2f0a4409 149 ros::Subscriber<geometry_msgs::Vector3> s("feedback_wheel_angularVel",messageCb);
tea5062001 0:c94b2f0a4409 150
tea5062001 0:c94b2f0a4409 151 int update_source = 0;
tea5062001 0:c94b2f0a4409 152
tea5062001 0:c94b2f0a4409 153 // subscribe point
tea5062001 0:c94b2f0a4409 154 void messageCb2(const geometry_msgs::Vector3& pp)
tea5062001 0:c94b2f0a4409 155 {
tea5062001 0:c94b2f0a4409 156 px = pp.x;
tea5062001 0:c94b2f0a4409 157 py = pp.y;
tea5062001 0:c94b2f0a4409 158 Theta_c = pp.z;
tea5062001 0:c94b2f0a4409 159
tea5062001 0:c94b2f0a4409 160 if(px != 0 || py != 0 || Theta_c != 0)
tea5062001 0:c94b2f0a4409 161 vision_flag = 1;
tea5062001 0:c94b2f0a4409 162 else
tea5062001 0:c94b2f0a4409 163 vision_flag = 0;
tea5062001 0:c94b2f0a4409 164
tea5062001 0:c94b2f0a4409 165 if(vision_flag == 1 && px > 0 && py > 0)
tea5062001 0:c94b2f0a4409 166 {
tea5062001 0:c94b2f0a4409 167 // from image plane coordinate to camera coordinate
tea5062001 0:c94b2f0a4409 168 Xc = (Zc/alpha_u)*(px - cx) - ((alpha_s*Zc)/(alpha_u*alpha_v))*(py - cy);
tea5062001 0:c94b2f0a4409 169 Yc = (Zc/alpha_v)*(py - cy);
tea5062001 0:c94b2f0a4409 170
tea5062001 0:c94b2f0a4409 171 // from camera coordinate to robot coordinate
tea5062001 0:c94b2f0a4409 172 Xr = -Yc + 175;
tea5062001 0:c94b2f0a4409 173 Yr = -Xc - 10;
tea5062001 0:c94b2f0a4409 174
tea5062001 0:c94b2f0a4409 175 // change unit to meter
tea5062001 0:c94b2f0a4409 176 Xr = Xr * 0.001;
tea5062001 0:c94b2f0a4409 177 Yr = Yr * 0.001;
tea5062001 0:c94b2f0a4409 178
tea5062001 0:c94b2f0a4409 179 update_source = 1;
tea5062001 0:c94b2f0a4409 180 }
tea5062001 0:c94b2f0a4409 181 else if(vision_flag == 1 && px == 0 && py == 0 && ((Theta_c >= 1 && Theta_c <= 89) || (Theta_c >= -89 && Theta_c <= -1)))
tea5062001 0:c94b2f0a4409 182 {
tea5062001 0:c94b2f0a4409 183 update_source = 2;
tea5062001 0:c94b2f0a4409 184 }
tea5062001 0:c94b2f0a4409 185 else
tea5062001 0:c94b2f0a4409 186 {
tea5062001 0:c94b2f0a4409 187 Xr = 0;
tea5062001 0:c94b2f0a4409 188 Yr = 0;
tea5062001 0:c94b2f0a4409 189
tea5062001 0:c94b2f0a4409 190 update_source = 0;
tea5062001 0:c94b2f0a4409 191 }
tea5062001 0:c94b2f0a4409 192 }
tea5062001 0:c94b2f0a4409 193
tea5062001 0:c94b2f0a4409 194 ros::Subscriber<geometry_msgs::Vector3> s2("point",messageCb2);
tea5062001 0:c94b2f0a4409 195
tea5062001 0:c94b2f0a4409 196 void read_sensor(void);
tea5062001 0:c94b2f0a4409 197
tea5062001 0:c94b2f0a4409 198 float lpf(float input, float output_old, float frequency);
tea5062001 0:c94b2f0a4409 199
tea5062001 0:c94b2f0a4409 200 int sensor_flag = 0; // sensor initial flag
tea5062001 0:c94b2f0a4409 201 int sensor_count = 0;
tea5062001 0:c94b2f0a4409 202
tea5062001 0:c94b2f0a4409 203 // sensor filter correction data
tea5062001 0:c94b2f0a4409 204 float Gyro_axis_data_f[3];
tea5062001 0:c94b2f0a4409 205 float Gyro_axis_data_f_old[3];
tea5062001 0:c94b2f0a4409 206 float Gyro_scale[3]; // scale = (data - zero)
tea5062001 0:c94b2f0a4409 207 float Gyro_axis_zero[3] = {0,0,0}; // x,y no use
tea5062001 0:c94b2f0a4409 208
tea5062001 0:c94b2f0a4409 209 // sensor 1 (front)
tea5062001 0:c94b2f0a4409 210 float Magn_axis_data_f[3];
tea5062001 0:c94b2f0a4409 211 float Magn_axis_data_f_old[3];
tea5062001 0:c94b2f0a4409 212 float Magn_scale[3]; // scale = (data - zero)
tea5062001 0:c94b2f0a4409 213 float Magn_axis_zero[3] = {-0.1049,0.1982,0};
tea5062001 0:c94b2f0a4409 214
tea5062001 0:c94b2f0a4409 215 // sensor 2 (back)
tea5062001 0:c94b2f0a4409 216 float Magn_axis_data_f2[3];
tea5062001 0:c94b2f0a4409 217 float Magn_axis_data_f_old2[3];
tea5062001 0:c94b2f0a4409 218 float Magn_scale2[3]; // scale = (data - zero)
tea5062001 0:c94b2f0a4409 219 float Magn_axis_zero2[3] = {0.1547,0.2713,0};
tea5062001 0:c94b2f0a4409 220
tea5062001 0:c94b2f0a4409 221 float Bex = 0;
tea5062001 0:c94b2f0a4409 222 float Bey = 0;
tea5062001 0:c94b2f0a4409 223 float Bex2 = 0;
tea5062001 0:c94b2f0a4409 224 float Bey2 = 0;
tea5062001 0:c94b2f0a4409 225 float Be = 0.9882; // Be = ( sqrt(Bex*Bex + Bey*Bey) + sqrt(Bex2*Bex2 + Bey2*Bey2) ) / 2
tea5062001 0:c94b2f0a4409 226
tea5062001 0:c94b2f0a4409 227 // counter for gyro bias
tea5062001 0:c94b2f0a4409 228 int gyro_count = 0;
tea5062001 0:c94b2f0a4409 229 int gyro_flag = 0;
tea5062001 0:c94b2f0a4409 230 float gyro_init[10] = {0};
tea5062001 0:c94b2f0a4409 231 int index_count = 0;
tea5062001 0:c94b2f0a4409 232 float gyro_sum = 0;
tea5062001 0:c94b2f0a4409 233
tea5062001 0:c94b2f0a4409 234 // sensor data
tea5062001 0:c94b2f0a4409 235 // magn
tea5062001 0:c94b2f0a4409 236 float phi = 0;
tea5062001 0:c94b2f0a4409 237 float phi2 = 0;
tea5062001 0:c94b2f0a4409 238 int phi_init_flag = 0;
tea5062001 0:c94b2f0a4409 239 int phi_init_count = 0;
tea5062001 0:c94b2f0a4409 240 float phi_init = 0;
tea5062001 0:c94b2f0a4409 241 float phi_init2 = 0;
tea5062001 0:c94b2f0a4409 242 float phi_m = 0;
tea5062001 0:c94b2f0a4409 243 float phi_m2 = 0;
tea5062001 0:c94b2f0a4409 244 int phi_m_deg = 0;
tea5062001 0:c94b2f0a4409 245 int phi_m2_deg = 0;
tea5062001 0:c94b2f0a4409 246 int phi_m_deg_1 = 0;
tea5062001 0:c94b2f0a4409 247 int phi_m2_deg_1 = 0;
tea5062001 0:c94b2f0a4409 248 int phi_m_deg_2 = 0;
tea5062001 0:c94b2f0a4409 249 int phi_m2_deg_2 = 0;
tea5062001 0:c94b2f0a4409 250 int phi_m_deg_T = 0;
tea5062001 0:c94b2f0a4409 251 int phi_m2_deg_T = 0;
tea5062001 0:c94b2f0a4409 252 float phi_m_a = 0;
tea5062001 0:c94b2f0a4409 253 float phi_m2_a = 0;
tea5062001 0:c94b2f0a4409 254 int phi_m_a_deg = 0;
tea5062001 0:c94b2f0a4409 255 int phi_m2_a_deg = 0;
tea5062001 0:c94b2f0a4409 256 int diff_phi_count = 0;
tea5062001 0:c94b2f0a4409 257 int diff_phi_flag = 0;
tea5062001 0:c94b2f0a4409 258 int jump_count = 0;
tea5062001 0:c94b2f0a4409 259 int jump_count2 = 0;
tea5062001 0:c94b2f0a4409 260 float phi_m_a_avg = 0;
tea5062001 0:c94b2f0a4409 261
tea5062001 0:c94b2f0a4409 262 // gyro
tea5062001 0:c94b2f0a4409 263 float w_s = 0;
tea5062001 0:c94b2f0a4409 264 float w_s_d = 0;
tea5062001 0:c94b2f0a4409 265 float theta_s = 0;
tea5062001 0:c94b2f0a4409 266 int theta_s_deg = 0;
tea5062001 0:c94b2f0a4409 267
tea5062001 0:c94b2f0a4409 268 // fusion data for theta
tea5062001 0:c94b2f0a4409 269 float theta_fu = 0;
tea5062001 0:c94b2f0a4409 270 float theta_fu_old = 0;
tea5062001 0:c94b2f0a4409 271 float bias = 0;
tea5062001 0:c94b2f0a4409 272 float bias_old = 0;
tea5062001 0:c94b2f0a4409 273 float y = 0;
tea5062001 0:c94b2f0a4409 274 float L1 = 0;
tea5062001 0:c94b2f0a4409 275 float L2 = 0;
tea5062001 0:c94b2f0a4409 276 int fusion_flag = 0;
tea5062001 0:c94b2f0a4409 277
tea5062001 0:c94b2f0a4409 278 // theta_fu to ROS
tea5062001 0:c94b2f0a4409 279 int theta_fu_deg = 0;
tea5062001 0:c94b2f0a4409 280 int theta_fu_deg_180 = 0;
tea5062001 0:c94b2f0a4409 281 float theta_fu_PI = 0;
tea5062001 0:c94b2f0a4409 282
tea5062001 0:c94b2f0a4409 283 // add bias
tea5062001 0:c94b2f0a4409 284 int bias_flag = 0;
tea5062001 0:c94b2f0a4409 285 float bias_add = 0;
tea5062001 0:c94b2f0a4409 286
tea5062001 0:c94b2f0a4409 287 // magnetic field disturbance detect
tea5062001 0:c94b2f0a4409 288 // trust index
tea5062001 0:c94b2f0a4409 289 float Q = 0;
tea5062001 0:c94b2f0a4409 290 // Kalman filter
tea5062001 0:c94b2f0a4409 291 float V = 0; // measurement noise variance
tea5062001 0:c94b2f0a4409 292 float theta_me = 0;
tea5062001 0:c94b2f0a4409 293 int theta_me_deg = 0;
tea5062001 0:c94b2f0a4409 294 float theta_me_bar = 0;
tea5062001 0:c94b2f0a4409 295 float sigma = 0;
tea5062001 0:c94b2f0a4409 296 float sigma_bar = 0;
tea5062001 0:c94b2f0a4409 297 float K = 0; // gain
tea5062001 0:c94b2f0a4409 298 float At = 1;
tea5062001 0:c94b2f0a4409 299 float Bt = 0.002; // sampling time
tea5062001 0:c94b2f0a4409 300 float Ct = 1;
tea5062001 0:c94b2f0a4409 301 float Rt = 0.001; // process noise variance
tea5062001 0:c94b2f0a4409 302 float update_distance = 0;
tea5062001 0:c94b2f0a4409 303 float update_rotation = 0;
tea5062001 0:c94b2f0a4409 304 int update_flag = 0;
tea5062001 0:c94b2f0a4409 305
tea5062001 0:c94b2f0a4409 306 // extended kalman filter for localization
tea5062001 0:c94b2f0a4409 307 // EKF variables
tea5062001 0:c94b2f0a4409 308 // prediction variables
tea5062001 0:c94b2f0a4409 309 float mu_bar[3][1] = {0};
tea5062001 0:c94b2f0a4409 310 int mu_bar_deg = 0;
tea5062001 0:c94b2f0a4409 311 float mu[3][1] = {0}; // state vector
tea5062001 0:c94b2f0a4409 312 float Sigma_bar[3][3] = {0};
tea5062001 0:c94b2f0a4409 313 float Sigma[3][3] = { {0 , 0 , 0} , {0 , 0 , 0} , {0 , 0 , 0} }; // estimation covariance matrix
tea5062001 0:c94b2f0a4409 314 float Sigma_mul_1[3][3] = {0};
tea5062001 0:c94b2f0a4409 315 float Sigma_mul_2[3][3] = {0};
tea5062001 0:c94b2f0a4409 316 float G[3][3] = {0}; // state transition matrix
tea5062001 0:c94b2f0a4409 317 float GT[3][3] = {0}; // state transition matrix transport
tea5062001 0:c94b2f0a4409 318 float R[3][3] = { {0.000001 , 0 , 0} , {0 , 0.000001 , 0} , {0 , 0 , 0.000007} }; // process noise covariance matrix
tea5062001 0:c94b2f0a4409 319
tea5062001 0:c94b2f0a4409 320 // encoder odometry
tea5062001 0:c94b2f0a4409 321 float th_enc = 0;
tea5062001 0:c94b2f0a4409 322 int th_enc_deg = 0;
tea5062001 0:c94b2f0a4409 323 float x_enc = 0;
tea5062001 0:c94b2f0a4409 324 float y_enc = 0;
tea5062001 0:c94b2f0a4409 325
tea5062001 0:c94b2f0a4409 326 // global cross map variables
tea5062001 0:c94b2f0a4409 327 float block_length = 0.39; // m
tea5062001 0:c94b2f0a4409 328 float x_crom[4] = {0};
tea5062001 0:c94b2f0a4409 329 float y_crom[4] = {0};
tea5062001 0:c94b2f0a4409 330
tea5062001 0:c94b2f0a4409 331 // update variables for cross detection
tea5062001 0:c94b2f0a4409 332 float Zcro[2][1] = {0};
tea5062001 0:c94b2f0a4409 333 float Zr[4] = {0};
tea5062001 0:c94b2f0a4409 334 int Za[4] = {0};
tea5062001 0:c94b2f0a4409 335 int Zphi[4] = {0};
tea5062001 0:c94b2f0a4409 336 int Zphi_deg_180[4] = {0};
tea5062001 0:c94b2f0a4409 337 float Zcro_hat[4][2][1] = {0};
tea5062001 0:c94b2f0a4409 338 float Zd[4][2][1] = {0};
tea5062001 0:c94b2f0a4409 339 float Pcro[4] = {0};
tea5062001 0:c94b2f0a4409 340 float Pcro_max = 0;
tea5062001 0:c94b2f0a4409 341 int i_Pcro_max = 0;
tea5062001 0:c94b2f0a4409 342 float e_r[4] = {0};
tea5062001 0:c94b2f0a4409 343 float mx_x[4] = {0};
tea5062001 0:c94b2f0a4409 344 float my_y[4] = {0};
tea5062001 0:c94b2f0a4409 345 float Zcro_hs[2][1] = {0};
tea5062001 0:c94b2f0a4409 346 float Hcro[4][2][3] = {0};
tea5062001 0:c94b2f0a4409 347 float Hcros[2][3] = {0};
tea5062001 0:c94b2f0a4409 348 float HcroT[4][3][2] = {0};
tea5062001 0:c94b2f0a4409 349 float HcroTs[3][2] = {0};
tea5062001 0:c94b2f0a4409 350 float Qcro[2][2] = {0};
tea5062001 0:c94b2f0a4409 351 float Scro[4][2][2] = {0};
tea5062001 0:c94b2f0a4409 352 float det_Scro[4] = {0};
tea5062001 0:c94b2f0a4409 353 float Scro_inv[4][2][2] = {0};
tea5062001 0:c94b2f0a4409 354 float Scro_is[2][2] = {0};
tea5062001 0:c94b2f0a4409 355 float Kcro[3][2] = {0};
tea5062001 0:c94b2f0a4409 356
tea5062001 0:c94b2f0a4409 357 float Hcro_1[4][2][3] = {0};
tea5062001 0:c94b2f0a4409 358 float Hcro_2[4][2][2] = {0};
tea5062001 0:c94b2f0a4409 359 float Zcro_delta[2][1] = {0};
tea5062001 0:c94b2f0a4409 360 float Kcro_1[3][2] = {0};
tea5062001 0:c94b2f0a4409 361 float Kcro_2[3][1] = {0};
tea5062001 0:c94b2f0a4409 362 float Icro_1[3][3] = {0};
tea5062001 0:c94b2f0a4409 363 float Icro_2[3][3] = {0};
tea5062001 0:c94b2f0a4409 364
tea5062001 0:c94b2f0a4409 365 // update variables for line detection
tea5062001 0:c94b2f0a4409 366 float Zli = 0;
tea5062001 0:c94b2f0a4409 367 float Zli_hat[4] = {0};
tea5062001 0:c94b2f0a4409 368 float Zli_diff[4] = {0};
tea5062001 0:c94b2f0a4409 369 float Zli_diff_min = 0;
tea5062001 0:c94b2f0a4409 370 int ind_diff_min = 0;
tea5062001 0:c94b2f0a4409 371 float Qli = 0.001;
tea5062001 0:c94b2f0a4409 372 float Sli = 0;
tea5062001 0:c94b2f0a4409 373 float Kli[3][1] = {0};
tea5062001 0:c94b2f0a4409 374
tea5062001 0:c94b2f0a4409 375 // update occasion
tea5062001 0:c94b2f0a4409 376 int camera_flag = 0;
tea5062001 0:c94b2f0a4409 377 int update_state = 0;
tea5062001 0:c94b2f0a4409 378 int singular_flag = 0;
tea5062001 0:c94b2f0a4409 379 int predict_count = 0;
tea5062001 0:c94b2f0a4409 380 int predict_flag = 0;
tea5062001 0:c94b2f0a4409 381 float up_dis = 0;
tea5062001 0:c94b2f0a4409 382 float up_rot = 0;
tea5062001 0:c94b2f0a4409 383 int up_check = 0;
tea5062001 0:c94b2f0a4409 384 float up_dis_r = 0.002;
tea5062001 0:c94b2f0a4409 385 float up_rot_r = PI/60;
tea5062001 0:c94b2f0a4409 386
tea5062001 0:c94b2f0a4409 387 // encoder magnetometer gyro odometry
tea5062001 0:c94b2f0a4409 388 float th_emg = 0;
tea5062001 0:c94b2f0a4409 389 int th_emg_deg = 0;
tea5062001 0:c94b2f0a4409 390 float x_emg = 0;
tea5062001 0:c94b2f0a4409 391 float y_emg = 0;
tea5062001 0:c94b2f0a4409 392
tea5062001 0:c94b2f0a4409 393 // select update source for debugging
tea5062001 0:c94b2f0a4409 394 // 0 for no update , 1 for cross update only , 2 for line update only , 3 for cross and line update together
tea5062001 0:c94b2f0a4409 395 int select_ups = 3;
tea5062001 0:c94b2f0a4409 396
tea5062001 0:c94b2f0a4409 397 // select prediction model for debugging
tea5062001 0:c94b2f0a4409 398 // 0 for encoder magnetometer gyro fusion , 1 for encoder only
tea5062001 0:c94b2f0a4409 399 int select_pms = 1;
tea5062001 0:c94b2f0a4409 400
tea5062001 0:c94b2f0a4409 401 // navigation
tea5062001 0:c94b2f0a4409 402 int mu_th_deg = 0;
tea5062001 0:c94b2f0a4409 403 int mu_th_deg_180 = 0;
tea5062001 0:c94b2f0a4409 404 float mu_th_PI = 0;
tea5062001 0:c94b2f0a4409 405 float Pxd = 0;
tea5062001 0:c94b2f0a4409 406 float Pyd = 0;
tea5062001 0:c94b2f0a4409 407 float Od = 0;
tea5062001 0:c94b2f0a4409 408 float delta_Pxd = 0;
tea5062001 0:c94b2f0a4409 409 float delta_Pyd = 0;
tea5062001 0:c94b2f0a4409 410 float delta_Od = 0;
tea5062001 0:c94b2f0a4409 411 float Kv = 0.55;
tea5062001 0:c94b2f0a4409 412 float Kw = 0.55;
tea5062001 0:c94b2f0a4409 413 float gain_v = 0;
tea5062001 0:c94b2f0a4409 414 float gain_w = 0;
tea5062001 0:c94b2f0a4409 415 float error_P = 0;
tea5062001 0:c94b2f0a4409 416 float error_O = 0;
tea5062001 0:c94b2f0a4409 417 int nav_flag = 0;
tea5062001 0:c94b2f0a4409 418 int nav_state = 1;
tea5062001 0:c94b2f0a4409 419 float distance_error_P = 0;
tea5062001 0:c94b2f0a4409 420 int line_tracker = 0;
tea5062001 0:c94b2f0a4409 421
tea5062001 0:c94b2f0a4409 422
tea5062001 0:c94b2f0a4409 423 int main()
tea5062001 0:c94b2f0a4409 424 {
tea5062001 0:c94b2f0a4409 425 t.start();
tea5062001 0:c94b2f0a4409 426
tea5062001 0:c94b2f0a4409 427 nh.initNode();
tea5062001 0:c94b2f0a4409 428
tea5062001 0:c94b2f0a4409 429 nh.subscribe(s);
tea5062001 0:c94b2f0a4409 430 nh.subscribe(s2);
tea5062001 0:c94b2f0a4409 431 nh.advertise(p);
tea5062001 0:c94b2f0a4409 432
tea5062001 0:c94b2f0a4409 433 init_uart();
tea5062001 0:c94b2f0a4409 434
tea5062001 0:c94b2f0a4409 435 init_IMU();
tea5062001 0:c94b2f0a4409 436
tea5062001 0:c94b2f0a4409 437 myled = 1;
tea5062001 0:c94b2f0a4409 438
tea5062001 0:c94b2f0a4409 439 while(1)
tea5062001 0:c94b2f0a4409 440 {
tea5062001 0:c94b2f0a4409 441 // receive labview data
tea5062001 0:c94b2f0a4409 442 if(uart_1.readable() > 0)
tea5062001 0:c94b2f0a4409 443 {
tea5062001 0:c94b2f0a4409 444 switch(uart_1.getc())
tea5062001 0:c94b2f0a4409 445 {
tea5062001 0:c94b2f0a4409 446 case 'a':
tea5062001 0:c94b2f0a4409 447 check_flag = 1;
tea5062001 0:c94b2f0a4409 448 BK_ = 0; // light
tea5062001 0:c94b2f0a4409 449 break;
tea5062001 0:c94b2f0a4409 450
tea5062001 0:c94b2f0a4409 451 case 's':
tea5062001 0:c94b2f0a4409 452 check_flag = 0;
tea5062001 0:c94b2f0a4409 453 BK_ = 1; // dark
tea5062001 0:c94b2f0a4409 454 break;
tea5062001 0:c94b2f0a4409 455
tea5062001 0:c94b2f0a4409 456 case 'z':
tea5062001 0:c94b2f0a4409 457 phi_init_flag = 1;
tea5062001 0:c94b2f0a4409 458 B_ = 0;
tea5062001 0:c94b2f0a4409 459 break;
tea5062001 0:c94b2f0a4409 460
tea5062001 0:c94b2f0a4409 461 case 'x':
tea5062001 0:c94b2f0a4409 462 phi_init_flag = 0;
tea5062001 0:c94b2f0a4409 463 B_ = 1;
tea5062001 0:c94b2f0a4409 464 break;
tea5062001 0:c94b2f0a4409 465
tea5062001 0:c94b2f0a4409 466 case 'b':
tea5062001 0:c94b2f0a4409 467 bias_flag = 1;
tea5062001 0:c94b2f0a4409 468 break;
tea5062001 0:c94b2f0a4409 469
tea5062001 0:c94b2f0a4409 470 case 'n':
tea5062001 0:c94b2f0a4409 471 bias_flag = 0;
tea5062001 0:c94b2f0a4409 472 break;
tea5062001 0:c94b2f0a4409 473
tea5062001 0:c94b2f0a4409 474 case 'c':
tea5062001 0:c94b2f0a4409 475 camera_flag = 1;
tea5062001 0:c94b2f0a4409 476 break;
tea5062001 0:c94b2f0a4409 477
tea5062001 0:c94b2f0a4409 478 case 'v':
tea5062001 0:c94b2f0a4409 479 camera_flag = 0;
tea5062001 0:c94b2f0a4409 480 break;
tea5062001 0:c94b2f0a4409 481
tea5062001 0:c94b2f0a4409 482 case 'h':
tea5062001 0:c94b2f0a4409 483 select_ups = 0;
tea5062001 0:c94b2f0a4409 484 break;
tea5062001 0:c94b2f0a4409 485
tea5062001 0:c94b2f0a4409 486 case 'j':
tea5062001 0:c94b2f0a4409 487 select_ups = 1;
tea5062001 0:c94b2f0a4409 488 break;
tea5062001 0:c94b2f0a4409 489
tea5062001 0:c94b2f0a4409 490 case 'k':
tea5062001 0:c94b2f0a4409 491 select_ups = 2;
tea5062001 0:c94b2f0a4409 492 break;
tea5062001 0:c94b2f0a4409 493
tea5062001 0:c94b2f0a4409 494 case 'l':
tea5062001 0:c94b2f0a4409 495 select_ups = 3;
tea5062001 0:c94b2f0a4409 496 break;
tea5062001 0:c94b2f0a4409 497
tea5062001 0:c94b2f0a4409 498 case 'y':
tea5062001 0:c94b2f0a4409 499 nav_flag = 1;
tea5062001 0:c94b2f0a4409 500 break;
tea5062001 0:c94b2f0a4409 501
tea5062001 0:c94b2f0a4409 502 case 'u':
tea5062001 0:c94b2f0a4409 503 nav_flag = 0;
tea5062001 0:c94b2f0a4409 504 break;
tea5062001 0:c94b2f0a4409 505
tea5062001 0:c94b2f0a4409 506 default:
tea5062001 0:c94b2f0a4409 507 check_flag = 0;
tea5062001 0:c94b2f0a4409 508 phi_init_flag = 0;
tea5062001 0:c94b2f0a4409 509 fusion_flag = 0;
tea5062001 0:c94b2f0a4409 510 bias_flag = 0;
tea5062001 0:c94b2f0a4409 511 camera_flag = 0;
tea5062001 0:c94b2f0a4409 512 select_ups = 0;
tea5062001 0:c94b2f0a4409 513 nav_flag = 0;
tea5062001 0:c94b2f0a4409 514 break;
tea5062001 0:c94b2f0a4409 515 }
tea5062001 0:c94b2f0a4409 516 }
tea5062001 0:c94b2f0a4409 517
tea5062001 0:c94b2f0a4409 518 // timer 1 : sensor fusion
tea5062001 0:c94b2f0a4409 519 if((t.read_us() - lastMilli) >= LOOPTIME) // 2000 us = 0.002 s
tea5062001 0:c94b2f0a4409 520 {
tea5062001 0:c94b2f0a4409 521 //myled = !myled;
tea5062001 0:c94b2f0a4409 522
tea5062001 0:c94b2f0a4409 523 lastMilli = t.read_us();
tea5062001 0:c94b2f0a4409 524
tea5062001 0:c94b2f0a4409 525 // sensor initial start
tea5062001 0:c94b2f0a4409 526 if(sensor_flag == 0)
tea5062001 0:c94b2f0a4409 527 {
tea5062001 0:c94b2f0a4409 528 sensor_count++;
tea5062001 0:c94b2f0a4409 529
tea5062001 0:c94b2f0a4409 530 if(sensor_count >= 100)
tea5062001 0:c94b2f0a4409 531 {
tea5062001 0:c94b2f0a4409 532 sensor_flag = 1;
tea5062001 0:c94b2f0a4409 533 sensor_count = 0;
tea5062001 0:c94b2f0a4409 534 }
tea5062001 0:c94b2f0a4409 535 }
tea5062001 0:c94b2f0a4409 536 else
tea5062001 0:c94b2f0a4409 537 {
tea5062001 0:c94b2f0a4409 538 read_sensor();
tea5062001 0:c94b2f0a4409 539
tea5062001 0:c94b2f0a4409 540 ////////////////// uart send data //////////////////////////////////////////////////////////////////////////////////////////////
tea5062001 0:c94b2f0a4409 541
tea5062001 0:c94b2f0a4409 542 display[0] = 1 * nav_flag;
tea5062001 0:c94b2f0a4409 543 display[1] = 1 * nav_state;
tea5062001 0:c94b2f0a4409 544 display[2] = 1 * up_check;
tea5062001 0:c94b2f0a4409 545 display[3] = 100 * error_P;
tea5062001 0:c94b2f0a4409 546 display[4] = 57.3 * error_O;
tea5062001 0:c94b2f0a4409 547 display[5] = 100 * distance_error_P;
tea5062001 0:c94b2f0a4409 548 display[6] = 100 * mu[0][0];
tea5062001 0:c94b2f0a4409 549 display[7] = 100 * mu[1][0];
tea5062001 0:c94b2f0a4409 550 display[8] = 57.3 * mu[2][0];
tea5062001 0:c94b2f0a4409 551 display[9] = 100 * x_enc;
tea5062001 0:c94b2f0a4409 552 display[10] = 100 * y_enc;
tea5062001 0:c94b2f0a4409 553 display[11] = 57.3 * th_enc;
tea5062001 0:c94b2f0a4409 554
tea5062001 0:c94b2f0a4409 555 // display[0] = 1000 * Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 556 // display[1] = 1000 * Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 557 // display[2] = up_check;
tea5062001 0:c94b2f0a4409 558 // display[3] = 100 * Zcro[0][0];
tea5062001 0:c94b2f0a4409 559 // display[4] = 100 * Zcro_hat[i_Pcro_max][0][0];
tea5062001 0:c94b2f0a4409 560 // display[5] = i_Pcro_max;
tea5062001 0:c94b2f0a4409 561 // display[6] = 57.3 * Zcro[1][0];
tea5062001 0:c94b2f0a4409 562 // display[7] = 57.3 * Zcro_hat[i_Pcro_max][1][0];
tea5062001 0:c94b2f0a4409 563 // display[8] = Pcro_max;
tea5062001 0:c94b2f0a4409 564 // display[9] = 100 * mu[0][0];
tea5062001 0:c94b2f0a4409 565 // display[10] = 100 * mu[1][0];
tea5062001 0:c94b2f0a4409 566 // display[11] = 57.3 * mu[2][0];
tea5062001 0:c94b2f0a4409 567
tea5062001 0:c94b2f0a4409 568 // display[0] = 100 * x_crom[0];
tea5062001 0:c94b2f0a4409 569 // display[1] = 100 * y_crom[0];
tea5062001 0:c94b2f0a4409 570 // display[2] = i_Pcro_max;
tea5062001 0:c94b2f0a4409 571 // display[3] = 100 * x_crom[1];
tea5062001 0:c94b2f0a4409 572 // display[4] = 100 * y_crom[1];
tea5062001 0:c94b2f0a4409 573 // display[5] = Pcro_max;
tea5062001 0:c94b2f0a4409 574 // display[6] = 100 * x_crom[2];
tea5062001 0:c94b2f0a4409 575 // display[7] = 100 * y_crom[2];
tea5062001 0:c94b2f0a4409 576 // display[8] = 100 * Xp;
tea5062001 0:c94b2f0a4409 577 // display[9] = 100 * x_crom[3];
tea5062001 0:c94b2f0a4409 578 // display[10] = 100 * y_crom[3];
tea5062001 0:c94b2f0a4409 579 // display[11] = 100 * Yp;
tea5062001 0:c94b2f0a4409 580
tea5062001 0:c94b2f0a4409 581 // display[0] = 10000 * Magn_axis_zero[0];
tea5062001 0:c94b2f0a4409 582 // display[1] = 10000 * Magn_axis_zero[1];
tea5062001 0:c94b2f0a4409 583 // display[2] = 10000 * a;
tea5062001 0:c94b2f0a4409 584 // display[3] = 10000 * b;
tea5062001 0:c94b2f0a4409 585 // display[4] = 10000 * Magn_axis_zero2[0];
tea5062001 0:c94b2f0a4409 586 // display[5] = 10000 * Magn_axis_zero2[1];
tea5062001 0:c94b2f0a4409 587 // display[6] = 10000 * a2;
tea5062001 0:c94b2f0a4409 588 // display[7] = 10000 * b2;
tea5062001 0:c94b2f0a4409 589 // display[8] = 10000 * Be;
tea5062001 0:c94b2f0a4409 590 // display[9] = 100 * Xr;
tea5062001 0:c94b2f0a4409 591 // display[10] = 100 * Yr;
tea5062001 0:c94b2f0a4409 592 // display[11] = 57.3 * th_enc;
tea5062001 0:c94b2f0a4409 593
tea5062001 0:c94b2f0a4409 594 // display[0] = 57.3 * phi_m_a;
tea5062001 0:c94b2f0a4409 595 // display[1] = 57.3 * phi_m2_a;
tea5062001 0:c94b2f0a4409 596 // display[2] = 57.3 * (0.5*phi_m_a + 0.5*phi_m2_a);
tea5062001 0:c94b2f0a4409 597 // display[3] = 57.3 * theta_fu;
tea5062001 0:c94b2f0a4409 598 // display[4] = 57.3 * theta_s;
tea5062001 0:c94b2f0a4409 599 // display[5] = 10000 * Q;
tea5062001 0:c94b2f0a4409 600 // display[6] = 57.3 * theta_me;
tea5062001 0:c94b2f0a4409 601 // display[7] = 57.3 * th_enc;
tea5062001 0:c94b2f0a4409 602 // display[8] = 57.3 * phi_m_a_avg;
tea5062001 0:c94b2f0a4409 603 // display[9] = 0;
tea5062001 0:c94b2f0a4409 604 // display[10] = 0;
tea5062001 0:c94b2f0a4409 605 // display[11] = 0;
tea5062001 0:c94b2f0a4409 606
tea5062001 0:c94b2f0a4409 607 ////////////////// uart send data //////////////////////////////////////////////////////////////////////////////////////////////
tea5062001 0:c94b2f0a4409 608
tea5062001 0:c94b2f0a4409 609 uart_send(display);
tea5062001 0:c94b2f0a4409 610 }
tea5062001 0:c94b2f0a4409 611 }
tea5062001 0:c94b2f0a4409 612
tea5062001 0:c94b2f0a4409 613 nh.spinOnce();
tea5062001 0:c94b2f0a4409 614
tea5062001 0:c94b2f0a4409 615 // timer 2 : publish cmd_vel for mag_cor
tea5062001 0:c94b2f0a4409 616 if((t.read_us() - lastMilli2) >= LOOPTIME2) // 100000 us = 0.1 s
tea5062001 0:c94b2f0a4409 617 {
tea5062001 0:c94b2f0a4409 618 lastMilli2 = t.read_us();
tea5062001 0:c94b2f0a4409 619
tea5062001 0:c94b2f0a4409 620 if(check_flag == 1)
tea5062001 0:c94b2f0a4409 621 {
tea5062001 0:c94b2f0a4409 622 vel_msg.angular.z = 0.35;
tea5062001 0:c94b2f0a4409 623 p.publish(&vel_msg);
tea5062001 0:c94b2f0a4409 624 }
tea5062001 0:c94b2f0a4409 625 }
tea5062001 0:c94b2f0a4409 626
tea5062001 0:c94b2f0a4409 627 // timer 3 : take part of mag data
tea5062001 0:c94b2f0a4409 628 if((t.read_us() - lastMilli3) >= LOOPTIME3) // 300000 us = 0.3 s
tea5062001 0:c94b2f0a4409 629 {
tea5062001 0:c94b2f0a4409 630 lastMilli3 = t.read_us();
tea5062001 0:c94b2f0a4409 631
tea5062001 0:c94b2f0a4409 632 if(check_flag == 1)
tea5062001 0:c94b2f0a4409 633 {
tea5062001 0:c94b2f0a4409 634 if(th >= PI/12) // after magn data stable then take data
tea5062001 0:c94b2f0a4409 635 {
tea5062001 0:c94b2f0a4409 636 magx[ind] = Magn_axis_data_f[0];
tea5062001 0:c94b2f0a4409 637 magy[ind] = Magn_axis_data_f[1];
tea5062001 0:c94b2f0a4409 638
tea5062001 0:c94b2f0a4409 639 magx2[ind] = Magn_axis_data_f2[0];
tea5062001 0:c94b2f0a4409 640 magy2[ind] = Magn_axis_data_f2[1];
tea5062001 0:c94b2f0a4409 641
tea5062001 0:c94b2f0a4409 642 ind++;
tea5062001 0:c94b2f0a4409 643 if(ind >= 100)
tea5062001 0:c94b2f0a4409 644 ind = 0;
tea5062001 0:c94b2f0a4409 645
tea5062001 0:c94b2f0a4409 646 ind_ = ind - 1; // send data index
tea5062001 0:c94b2f0a4409 647 }
tea5062001 0:c94b2f0a4409 648 }
tea5062001 0:c94b2f0a4409 649 }
tea5062001 0:c94b2f0a4409 650
tea5062001 0:c94b2f0a4409 651 ////////// timer 5 : extended kalman filter ////////////////////////////////////////////////////////////////////////////////////
tea5062001 0:c94b2f0a4409 652
tea5062001 0:c94b2f0a4409 653 if((t.read_us() - lastMilli5) >= LOOPTIME5) // 20000 us = 0.02 s
tea5062001 0:c94b2f0a4409 654 {
tea5062001 0:c94b2f0a4409 655 myled = !myled;
tea5062001 0:c94b2f0a4409 656
tea5062001 0:c94b2f0a4409 657 lastMilli5 = t.read_us();
tea5062001 0:c94b2f0a4409 658
tea5062001 0:c94b2f0a4409 659 if(fusion_flag == 1)
tea5062001 0:c94b2f0a4409 660 {
tea5062001 0:c94b2f0a4409 661 // extended kalman filter for robot localization
tea5062001 0:c94b2f0a4409 662
tea5062001 0:c94b2f0a4409 663 // *** Prediction Start *************************************************************** //
tea5062001 0:c94b2f0a4409 664
tea5062001 0:c94b2f0a4409 665 // encoder odometry
tea5062001 0:c94b2f0a4409 666 th_enc = th_enc + omega_z*T2;
tea5062001 0:c94b2f0a4409 667 x_enc = x_enc + vel_x*cos(th_enc)*T2;
tea5062001 0:c94b2f0a4409 668 y_enc = y_enc + vel_x*sin(th_enc)*T2;
tea5062001 0:c94b2f0a4409 669
tea5062001 0:c94b2f0a4409 670 // th_enc_deg range 0~359 deg
tea5062001 0:c94b2f0a4409 671 th_enc_deg = th_enc * 180/PI;
tea5062001 0:c94b2f0a4409 672
tea5062001 0:c94b2f0a4409 673 if(th_enc_deg < 0)
tea5062001 0:c94b2f0a4409 674 th_enc_deg = th_enc_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 675 else
tea5062001 0:c94b2f0a4409 676 th_enc_deg = th_enc_deg % 360;
tea5062001 0:c94b2f0a4409 677
tea5062001 0:c94b2f0a4409 678 // encoder magnetometer gyro odometry
tea5062001 0:c94b2f0a4409 679 th_emg = theta_fu;
tea5062001 0:c94b2f0a4409 680 x_emg = x_emg + vel_x*cos(th_emg)*T2;
tea5062001 0:c94b2f0a4409 681 y_emg = y_emg + vel_x*sin(th_emg)*T2;
tea5062001 0:c94b2f0a4409 682
tea5062001 0:c94b2f0a4409 683 // th_emg_deg range 0~359 deg
tea5062001 0:c94b2f0a4409 684 th_emg_deg = th_emg * 180/PI;
tea5062001 0:c94b2f0a4409 685
tea5062001 0:c94b2f0a4409 686 if(th_emg_deg < 0)
tea5062001 0:c94b2f0a4409 687 th_emg_deg = th_emg_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 688 else
tea5062001 0:c94b2f0a4409 689 th_emg_deg = th_emg_deg % 360;
tea5062001 0:c94b2f0a4409 690
tea5062001 0:c94b2f0a4409 691 // update distance and rotation angle
tea5062001 0:c94b2f0a4409 692 up_dis = up_dis + T2*vel_x;
tea5062001 0:c94b2f0a4409 693 up_rot = up_rot + T2*omega_z;
tea5062001 0:c94b2f0a4409 694
tea5062001 0:c94b2f0a4409 695 // mu_bar = g(u,mu)
tea5062001 0:c94b2f0a4409 696 switch(select_pms)
tea5062001 0:c94b2f0a4409 697 {
tea5062001 0:c94b2f0a4409 698 // 0 for encoder magnetometer gyro fusion
tea5062001 0:c94b2f0a4409 699 case 0:
tea5062001 0:c94b2f0a4409 700 mu_bar[0][0] = mu[0][0] + vel_x*cos(mu[2][0])*T2;
tea5062001 0:c94b2f0a4409 701 mu_bar[1][0] = mu[1][0] + vel_x*sin(mu[2][0])*T2;
tea5062001 0:c94b2f0a4409 702 mu_bar[2][0] = theta_fu;
tea5062001 0:c94b2f0a4409 703
tea5062001 0:c94b2f0a4409 704 mu_bar_deg = theta_fu_deg;
tea5062001 0:c94b2f0a4409 705 break;
tea5062001 0:c94b2f0a4409 706
tea5062001 0:c94b2f0a4409 707 // 1 for encoder only
tea5062001 0:c94b2f0a4409 708 case 1:
tea5062001 0:c94b2f0a4409 709 mu_bar[0][0] = mu[0][0] + vel_x*cos(mu[2][0])*T2;
tea5062001 0:c94b2f0a4409 710 mu_bar[1][0] = mu[1][0] + vel_x*sin(mu[2][0])*T2;
tea5062001 0:c94b2f0a4409 711 mu_bar[2][0] = mu[2][0] + omega_z*T2;
tea5062001 0:c94b2f0a4409 712
tea5062001 0:c94b2f0a4409 713 // mu_bar_deg range 0~359 deg
tea5062001 0:c94b2f0a4409 714 mu_bar_deg = mu_bar[2][0] * 180/PI;
tea5062001 0:c94b2f0a4409 715
tea5062001 0:c94b2f0a4409 716 if(mu_bar_deg < 0)
tea5062001 0:c94b2f0a4409 717 mu_bar_deg = mu_bar_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 718 else
tea5062001 0:c94b2f0a4409 719 mu_bar_deg = mu_bar_deg % 360;
tea5062001 0:c94b2f0a4409 720 break;
tea5062001 0:c94b2f0a4409 721 }
tea5062001 0:c94b2f0a4409 722
tea5062001 0:c94b2f0a4409 723 // covariance prediction accumulation timing
tea5062001 0:c94b2f0a4409 724 if(vel_x != 0 || omega_z != 0)
tea5062001 0:c94b2f0a4409 725 {
tea5062001 0:c94b2f0a4409 726 predict_count++;
tea5062001 0:c94b2f0a4409 727 }
tea5062001 0:c94b2f0a4409 728
tea5062001 0:c94b2f0a4409 729 if(predict_count >= 25)
tea5062001 0:c94b2f0a4409 730 {
tea5062001 0:c94b2f0a4409 731 predict_flag = 1;
tea5062001 0:c94b2f0a4409 732
tea5062001 0:c94b2f0a4409 733 predict_count = 0;
tea5062001 0:c94b2f0a4409 734 }
tea5062001 0:c94b2f0a4409 735
tea5062001 0:c94b2f0a4409 736 if(predict_flag == 1)
tea5062001 0:c94b2f0a4409 737 {
tea5062001 0:c94b2f0a4409 738 // Sigma_bar = G*Sigma*G^T + R
tea5062001 0:c94b2f0a4409 739 G[0][0] = 1;
tea5062001 0:c94b2f0a4409 740 G[1][1] = 1;
tea5062001 0:c94b2f0a4409 741 G[2][2] = 1;
tea5062001 0:c94b2f0a4409 742 G[0][2] = -vel_x*sin(mu[2][0])*T2;
tea5062001 0:c94b2f0a4409 743 G[1][2] = vel_x*cos(mu[2][0])*T2;
tea5062001 0:c94b2f0a4409 744
tea5062001 0:c94b2f0a4409 745 // get GT
tea5062001 0:c94b2f0a4409 746 GT[0][0] = 1;
tea5062001 0:c94b2f0a4409 747 GT[1][1] = 1;
tea5062001 0:c94b2f0a4409 748 GT[2][2] = 1;
tea5062001 0:c94b2f0a4409 749 GT[2][0] = G[0][2];
tea5062001 0:c94b2f0a4409 750 GT[2][1] = G[1][2];
tea5062001 0:c94b2f0a4409 751
tea5062001 0:c94b2f0a4409 752 // G*Sigma = Sigma_mul_1
tea5062001 0:c94b2f0a4409 753 Sigma_mul_1[0][0] = G[0][0]*Sigma[0][0] + G[0][1]*Sigma[1][0] + G[0][2]*Sigma[2][0];
tea5062001 0:c94b2f0a4409 754 Sigma_mul_1[0][1] = G[0][0]*Sigma[0][1] + G[0][1]*Sigma[1][1] + G[0][2]*Sigma[2][1];
tea5062001 0:c94b2f0a4409 755 Sigma_mul_1[0][2] = G[0][0]*Sigma[0][2] + G[0][1]*Sigma[1][2] + G[0][2]*Sigma[2][2];
tea5062001 0:c94b2f0a4409 756 Sigma_mul_1[1][0] = G[1][0]*Sigma[0][0] + G[1][1]*Sigma[1][0] + G[1][2]*Sigma[2][0];
tea5062001 0:c94b2f0a4409 757 Sigma_mul_1[1][1] = G[1][0]*Sigma[0][1] + G[1][1]*Sigma[1][1] + G[1][2]*Sigma[2][1];
tea5062001 0:c94b2f0a4409 758 Sigma_mul_1[1][2] = G[1][0]*Sigma[0][2] + G[1][1]*Sigma[1][2] + G[1][2]*Sigma[2][2];
tea5062001 0:c94b2f0a4409 759 Sigma_mul_1[2][0] = G[2][0]*Sigma[0][0] + G[2][1]*Sigma[1][0] + G[2][2]*Sigma[2][0];
tea5062001 0:c94b2f0a4409 760 Sigma_mul_1[2][1] = G[2][0]*Sigma[0][1] + G[2][1]*Sigma[1][1] + G[2][2]*Sigma[2][1];
tea5062001 0:c94b2f0a4409 761 Sigma_mul_1[2][2] = G[2][0]*Sigma[0][2] + G[2][1]*Sigma[1][2] + G[2][2]*Sigma[2][2];
tea5062001 0:c94b2f0a4409 762
tea5062001 0:c94b2f0a4409 763 // G*Sigma * GT = Sigma_mul_1 * GT = Sigma_mul_2
tea5062001 0:c94b2f0a4409 764 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];
tea5062001 0:c94b2f0a4409 765 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];
tea5062001 0:c94b2f0a4409 766 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];
tea5062001 0:c94b2f0a4409 767 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];
tea5062001 0:c94b2f0a4409 768 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];
tea5062001 0:c94b2f0a4409 769 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];
tea5062001 0:c94b2f0a4409 770 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];
tea5062001 0:c94b2f0a4409 771 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];
tea5062001 0:c94b2f0a4409 772 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];
tea5062001 0:c94b2f0a4409 773
tea5062001 0:c94b2f0a4409 774 // G*Sigma*GT + R = Sigma_mul_2 + R = Sigma_bar
tea5062001 0:c94b2f0a4409 775 Sigma_bar[0][0] = Sigma_mul_2[0][0] + R[0][0];
tea5062001 0:c94b2f0a4409 776 Sigma_bar[0][1] = Sigma_mul_2[0][1] + R[0][1];
tea5062001 0:c94b2f0a4409 777 Sigma_bar[0][2] = Sigma_mul_2[0][2] + R[0][2];
tea5062001 0:c94b2f0a4409 778 Sigma_bar[1][0] = Sigma_mul_2[1][0] + R[1][0];
tea5062001 0:c94b2f0a4409 779 Sigma_bar[1][1] = Sigma_mul_2[1][1] + R[1][1];
tea5062001 0:c94b2f0a4409 780 Sigma_bar[1][2] = Sigma_mul_2[1][2] + R[1][2];
tea5062001 0:c94b2f0a4409 781 Sigma_bar[2][0] = Sigma_mul_2[2][0] + R[2][0];
tea5062001 0:c94b2f0a4409 782 Sigma_bar[2][1] = Sigma_mul_2[2][1] + R[2][1];
tea5062001 0:c94b2f0a4409 783 Sigma_bar[2][2] = Sigma_mul_2[2][2] + R[2][2];
tea5062001 0:c94b2f0a4409 784
tea5062001 0:c94b2f0a4409 785 predict_flag = 0;
tea5062001 0:c94b2f0a4409 786 }
tea5062001 0:c94b2f0a4409 787
tea5062001 0:c94b2f0a4409 788 // *** Prediction End ***************************************************************** //
tea5062001 0:c94b2f0a4409 789
tea5062001 0:c94b2f0a4409 790 // *** Update Start ******************************************************************* //
tea5062001 0:c94b2f0a4409 791
tea5062001 0:c94b2f0a4409 792 if(vision_flag == 1 && camera_flag == 1)
tea5062001 0:c94b2f0a4409 793 {
tea5062001 0:c94b2f0a4409 794 if(update_source == 1 && singular_flag == 0) // cross detection
tea5062001 0:c94b2f0a4409 795 {
tea5062001 0:c94b2f0a4409 796 // global cross map
tea5062001 0:c94b2f0a4409 797 Xp = mu_bar[0][0] + cos(mu_bar[2][0])*Xr - sin(mu_bar[2][0])*Yr;
tea5062001 0:c94b2f0a4409 798 Yp = mu_bar[1][0] + sin(mu_bar[2][0])*Xr + cos(mu_bar[2][0])*Yr;
tea5062001 0:c94b2f0a4409 799
tea5062001 0:c94b2f0a4409 800 if(Xp >= 0 && Yp >= 0)
tea5062001 0:c94b2f0a4409 801 {
tea5062001 0:c94b2f0a4409 802 // left down (0,0)
tea5062001 0:c94b2f0a4409 803 x_crom[0] = ((int)(Xp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 804 y_crom[0] = ((int)(Yp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 805 // right down (1,0)
tea5062001 0:c94b2f0a4409 806 x_crom[1] = ((int)(Xp/block_length) + 1) * block_length;
tea5062001 0:c94b2f0a4409 807 y_crom[1] = ((int)(Yp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 808 // right up (1,1)
tea5062001 0:c94b2f0a4409 809 x_crom[2] = ((int)(Xp/block_length) + 1) * block_length;
tea5062001 0:c94b2f0a4409 810 y_crom[2] = ((int)(Yp/block_length) + 1) * block_length;
tea5062001 0:c94b2f0a4409 811 // left up (0,1)
tea5062001 0:c94b2f0a4409 812 x_crom[3] = ((int)(Xp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 813 y_crom[3] = ((int)(Yp/block_length) + 1) * block_length;
tea5062001 0:c94b2f0a4409 814 }
tea5062001 0:c94b2f0a4409 815 else if(Xp < 0 && Yp >= 0)
tea5062001 0:c94b2f0a4409 816 {
tea5062001 0:c94b2f0a4409 817 // left down (-1,0)
tea5062001 0:c94b2f0a4409 818 x_crom[0] = ((int)(Xp/block_length) - 1) * block_length;
tea5062001 0:c94b2f0a4409 819 y_crom[0] = ((int)(Yp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 820 // right down (0,0)
tea5062001 0:c94b2f0a4409 821 x_crom[1] = ((int)(Xp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 822 y_crom[1] = ((int)(Yp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 823 // right up (0,1)
tea5062001 0:c94b2f0a4409 824 x_crom[2] = ((int)(Xp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 825 y_crom[2] = ((int)(Yp/block_length) + 1) * block_length;
tea5062001 0:c94b2f0a4409 826 // left up (-1,1)
tea5062001 0:c94b2f0a4409 827 x_crom[3] = ((int)(Xp/block_length) - 1) * block_length;
tea5062001 0:c94b2f0a4409 828 y_crom[3] = ((int)(Yp/block_length) + 1) * block_length;
tea5062001 0:c94b2f0a4409 829 }
tea5062001 0:c94b2f0a4409 830 else if(Xp < 0 && Yp < 0)
tea5062001 0:c94b2f0a4409 831 {
tea5062001 0:c94b2f0a4409 832 // left down (-1,-1)
tea5062001 0:c94b2f0a4409 833 x_crom[0] = ((int)(Xp/block_length) - 1) * block_length;
tea5062001 0:c94b2f0a4409 834 y_crom[0] = ((int)(Yp/block_length) - 1) * block_length;
tea5062001 0:c94b2f0a4409 835 // right down (0,-1)
tea5062001 0:c94b2f0a4409 836 x_crom[1] = ((int)(Xp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 837 y_crom[1] = ((int)(Yp/block_length) - 1) * block_length;
tea5062001 0:c94b2f0a4409 838 // right up (0,0)
tea5062001 0:c94b2f0a4409 839 x_crom[2] = ((int)(Xp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 840 y_crom[2] = ((int)(Yp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 841 // left up (-1,0)
tea5062001 0:c94b2f0a4409 842 x_crom[3] = ((int)(Xp/block_length) - 1) * block_length;
tea5062001 0:c94b2f0a4409 843 y_crom[3] = ((int)(Yp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 844 }
tea5062001 0:c94b2f0a4409 845 else if(Xp >= 0 && Yp < 0)
tea5062001 0:c94b2f0a4409 846 {
tea5062001 0:c94b2f0a4409 847 // left down (0,-1)
tea5062001 0:c94b2f0a4409 848 x_crom[0] = ((int)(Xp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 849 y_crom[0] = ((int)(Yp/block_length) - 1) * block_length;
tea5062001 0:c94b2f0a4409 850 // right down (1,-1)
tea5062001 0:c94b2f0a4409 851 x_crom[1] = ((int)(Xp/block_length) + 1) * block_length;
tea5062001 0:c94b2f0a4409 852 y_crom[1] = ((int)(Yp/block_length) - 1) * block_length;
tea5062001 0:c94b2f0a4409 853 // right up (1,0)
tea5062001 0:c94b2f0a4409 854 x_crom[2] = ((int)(Xp/block_length) + 1) * block_length;
tea5062001 0:c94b2f0a4409 855 y_crom[2] = ((int)(Yp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 856 // left up (0,0)
tea5062001 0:c94b2f0a4409 857 x_crom[3] = ((int)(Xp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 858 y_crom[3] = ((int)(Yp/block_length) + 0) * block_length;
tea5062001 0:c94b2f0a4409 859 }
tea5062001 0:c94b2f0a4409 860
tea5062001 0:c94b2f0a4409 861 // cross relative distance measurement , polar coordinates : (Zcro[0][0] , Zcro[1][0])
tea5062001 0:c94b2f0a4409 862 Zcro[0][0] = sqrt(pow(Xr,2) + pow(Yr,2));
tea5062001 0:c94b2f0a4409 863 Zcro[1][0] = atan2(Yr,Xr);
tea5062001 0:c94b2f0a4409 864
tea5062001 0:c94b2f0a4409 865 // cross relative distance prediction , polar coordinates : (Zr[i] , Zphi[i])
tea5062001 0:c94b2f0a4409 866 for(int i = 0 ; i < 4 ; i++)
tea5062001 0:c94b2f0a4409 867 {
tea5062001 0:c94b2f0a4409 868 Zr[i] = sqrt(pow((x_crom[i] - mu_bar[0][0]),2) + pow((y_crom[i] - mu_bar[1][0]),2));
tea5062001 0:c94b2f0a4409 869
tea5062001 0:c94b2f0a4409 870 Za[i] = atan2((y_crom[i] - mu_bar[1][0]),(x_crom[i] - mu_bar[0][0])) * 180/PI;
tea5062001 0:c94b2f0a4409 871
tea5062001 0:c94b2f0a4409 872 // Za[i] range 0~359 deg
tea5062001 0:c94b2f0a4409 873 if(Za[i] < 0)
tea5062001 0:c94b2f0a4409 874 Za[i] = Za[i] % 360 + 360;
tea5062001 0:c94b2f0a4409 875 else
tea5062001 0:c94b2f0a4409 876 Za[i] = Za[i] % 360;
tea5062001 0:c94b2f0a4409 877
tea5062001 0:c94b2f0a4409 878 Zphi[i] = Za[i] - mu_bar_deg;
tea5062001 0:c94b2f0a4409 879
tea5062001 0:c94b2f0a4409 880 // Zphi[i] range 0~359 deg
tea5062001 0:c94b2f0a4409 881 if(Zphi[i] < 0)
tea5062001 0:c94b2f0a4409 882 Zphi[i] = Zphi[i] % 360 + 360;
tea5062001 0:c94b2f0a4409 883 else
tea5062001 0:c94b2f0a4409 884 Zphi[i] = Zphi[i] % 360;
tea5062001 0:c94b2f0a4409 885
tea5062001 0:c94b2f0a4409 886 // Zphi_deg_180[i] range -179~180 deg
tea5062001 0:c94b2f0a4409 887 if(Zphi[i] > 180)
tea5062001 0:c94b2f0a4409 888 Zphi_deg_180[i] = Zphi[i] - 360;
tea5062001 0:c94b2f0a4409 889 else
tea5062001 0:c94b2f0a4409 890 Zphi_deg_180[i] = Zphi[i];
tea5062001 0:c94b2f0a4409 891
tea5062001 0:c94b2f0a4409 892 Zcro_hat[i][0][0] = Zr[i];
tea5062001 0:c94b2f0a4409 893 Zcro_hat[i][1][0] = Zphi_deg_180[i] * PI/180;
tea5062001 0:c94b2f0a4409 894 }
tea5062001 0:c94b2f0a4409 895
tea5062001 0:c94b2f0a4409 896 // cross measurement likelihood
tea5062001 0:c94b2f0a4409 897 for(int i = 0 ; i < 4 ; i++)
tea5062001 0:c94b2f0a4409 898 {
tea5062001 0:c94b2f0a4409 899 // measurement transition matrix element
tea5062001 0:c94b2f0a4409 900 if(Zr[i] != 0)
tea5062001 0:c94b2f0a4409 901 {
tea5062001 0:c94b2f0a4409 902 e_r[i] = 1 / Zr[i];
tea5062001 0:c94b2f0a4409 903
tea5062001 0:c94b2f0a4409 904 singular_flag = 0;
tea5062001 0:c94b2f0a4409 905 }
tea5062001 0:c94b2f0a4409 906 else
tea5062001 0:c94b2f0a4409 907 {
tea5062001 0:c94b2f0a4409 908 singular_flag = 1;
tea5062001 0:c94b2f0a4409 909 }
tea5062001 0:c94b2f0a4409 910
tea5062001 0:c94b2f0a4409 911 mx_x[i] = x_crom[i] - mu_bar[0][0];
tea5062001 0:c94b2f0a4409 912 my_y[i] = y_crom[i] - mu_bar[1][0];
tea5062001 0:c94b2f0a4409 913
tea5062001 0:c94b2f0a4409 914 Hcro[i][0][0] = -mx_x[i] * e_r[i];
tea5062001 0:c94b2f0a4409 915 Hcro[i][1][0] = my_y[i] * pow(e_r[i],2);
tea5062001 0:c94b2f0a4409 916 Hcro[i][0][1] = -my_y[i] * e_r[i];
tea5062001 0:c94b2f0a4409 917 Hcro[i][1][1] = -mx_x[i] * pow(e_r[i],2);
tea5062001 0:c94b2f0a4409 918 Hcro[i][0][2] = 0;
tea5062001 0:c94b2f0a4409 919 Hcro[i][1][2] = -1;
tea5062001 0:c94b2f0a4409 920
tea5062001 0:c94b2f0a4409 921 // get HcroT
tea5062001 0:c94b2f0a4409 922 HcroT[i][0][0] = Hcro[i][0][0];
tea5062001 0:c94b2f0a4409 923 HcroT[i][1][0] = Hcro[i][0][1];
tea5062001 0:c94b2f0a4409 924 HcroT[i][2][0] = Hcro[i][0][2];
tea5062001 0:c94b2f0a4409 925 HcroT[i][0][1] = Hcro[i][1][0];
tea5062001 0:c94b2f0a4409 926 HcroT[i][1][1] = Hcro[i][1][1];
tea5062001 0:c94b2f0a4409 927 HcroT[i][2][1] = Hcro[i][1][2];
tea5062001 0:c94b2f0a4409 928
tea5062001 0:c94b2f0a4409 929 // H*Sigma_bar = Hcro_1
tea5062001 0:c94b2f0a4409 930 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];
tea5062001 0:c94b2f0a4409 931 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];
tea5062001 0:c94b2f0a4409 932 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];
tea5062001 0:c94b2f0a4409 933 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];
tea5062001 0:c94b2f0a4409 934 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];
tea5062001 0:c94b2f0a4409 935 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];
tea5062001 0:c94b2f0a4409 936
tea5062001 0:c94b2f0a4409 937 // H*Sigma_bar * HT = Hcro_1 * HT = Hcro_2
tea5062001 0:c94b2f0a4409 938 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];
tea5062001 0:c94b2f0a4409 939 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];
tea5062001 0:c94b2f0a4409 940 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];
tea5062001 0:c94b2f0a4409 941 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];
tea5062001 0:c94b2f0a4409 942
tea5062001 0:c94b2f0a4409 943 Qcro[0][0] = 0.0001;
tea5062001 0:c94b2f0a4409 944 Qcro[1][1] = 0.001;
tea5062001 0:c94b2f0a4409 945
tea5062001 0:c94b2f0a4409 946 // H*Sigma_bar*HT + Q = Hcro_2 + Q = Scro
tea5062001 0:c94b2f0a4409 947 Scro[i][0][0] = Hcro_2[i][0][0] + Qcro[0][0];
tea5062001 0:c94b2f0a4409 948 Scro[i][0][1] = Hcro_2[i][0][1] + Qcro[0][1];
tea5062001 0:c94b2f0a4409 949 Scro[i][1][0] = Hcro_2[i][1][0] + Qcro[1][0];
tea5062001 0:c94b2f0a4409 950 Scro[i][1][1] = Hcro_2[i][1][1] + Qcro[1][1];
tea5062001 0:c94b2f0a4409 951
tea5062001 0:c94b2f0a4409 952 det_Scro[i] = Scro[i][0][0]*Scro[i][1][1] - Scro[i][0][1]*Scro[i][1][0];
tea5062001 0:c94b2f0a4409 953
tea5062001 0:c94b2f0a4409 954 if(det_Scro[i] != 0)
tea5062001 0:c94b2f0a4409 955 {
tea5062001 0:c94b2f0a4409 956 Scro_inv[i][0][0] = Scro[i][1][1] / det_Scro[i];
tea5062001 0:c94b2f0a4409 957 Scro_inv[i][0][1] = -Scro[i][0][1] / det_Scro[i];
tea5062001 0:c94b2f0a4409 958 Scro_inv[i][1][0] = -Scro[i][1][0] / det_Scro[i];
tea5062001 0:c94b2f0a4409 959 Scro_inv[i][1][1] = Scro[i][0][0] / det_Scro[i];
tea5062001 0:c94b2f0a4409 960
tea5062001 0:c94b2f0a4409 961 singular_flag = 0;
tea5062001 0:c94b2f0a4409 962 }
tea5062001 0:c94b2f0a4409 963 else
tea5062001 0:c94b2f0a4409 964 {
tea5062001 0:c94b2f0a4409 965 singular_flag = 1;
tea5062001 0:c94b2f0a4409 966 }
tea5062001 0:c94b2f0a4409 967
tea5062001 0:c94b2f0a4409 968 // measurement difference
tea5062001 0:c94b2f0a4409 969 Zd[i][0][0] = Zcro[0][0] - Zcro_hat[i][0][0];
tea5062001 0:c94b2f0a4409 970 Zd[i][1][0] = Zcro[1][0] - Zcro_hat[i][1][0];
tea5062001 0:c94b2f0a4409 971
tea5062001 0:c94b2f0a4409 972 // measurement likelihood
tea5062001 0:c94b2f0a4409 973 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]));
tea5062001 0:c94b2f0a4409 974 }
tea5062001 0:c94b2f0a4409 975
tea5062001 0:c94b2f0a4409 976 // find the maximum likelihood and index
tea5062001 0:c94b2f0a4409 977 Pcro_max = Pcro[0];
tea5062001 0:c94b2f0a4409 978 i_Pcro_max = 0;
tea5062001 0:c94b2f0a4409 979
tea5062001 0:c94b2f0a4409 980 for(int j = 1 ; j < 4 ; j++)
tea5062001 0:c94b2f0a4409 981 {
tea5062001 0:c94b2f0a4409 982 if(Pcro[j] > Pcro_max)
tea5062001 0:c94b2f0a4409 983 {
tea5062001 0:c94b2f0a4409 984 Pcro_max = Pcro[j];
tea5062001 0:c94b2f0a4409 985 i_Pcro_max = j;
tea5062001 0:c94b2f0a4409 986 }
tea5062001 0:c94b2f0a4409 987 }
tea5062001 0:c94b2f0a4409 988
tea5062001 0:c94b2f0a4409 989 // Z_hat select
tea5062001 0:c94b2f0a4409 990 Zcro_hs[0][0] = Zcro_hat[i_Pcro_max][0][0];
tea5062001 0:c94b2f0a4409 991 Zcro_hs[1][0] = Zcro_hat[i_Pcro_max][1][0];
tea5062001 0:c94b2f0a4409 992
tea5062001 0:c94b2f0a4409 993 // H select
tea5062001 0:c94b2f0a4409 994 Hcros[0][0] = Hcro[i_Pcro_max][0][0];
tea5062001 0:c94b2f0a4409 995 Hcros[0][1] = Hcro[i_Pcro_max][0][1];
tea5062001 0:c94b2f0a4409 996 Hcros[0][2] = Hcro[i_Pcro_max][0][2];
tea5062001 0:c94b2f0a4409 997 Hcros[1][0] = Hcro[i_Pcro_max][1][0];
tea5062001 0:c94b2f0a4409 998 Hcros[1][1] = Hcro[i_Pcro_max][1][1];
tea5062001 0:c94b2f0a4409 999 Hcros[1][2] = Hcro[i_Pcro_max][1][2];
tea5062001 0:c94b2f0a4409 1000
tea5062001 0:c94b2f0a4409 1001 // HT select
tea5062001 0:c94b2f0a4409 1002 HcroTs[0][0] = HcroT[i_Pcro_max][0][0];
tea5062001 0:c94b2f0a4409 1003 HcroTs[0][1] = HcroT[i_Pcro_max][0][1];
tea5062001 0:c94b2f0a4409 1004 HcroTs[1][0] = HcroT[i_Pcro_max][1][0];
tea5062001 0:c94b2f0a4409 1005 HcroTs[1][1] = HcroT[i_Pcro_max][1][1];
tea5062001 0:c94b2f0a4409 1006 HcroTs[2][0] = HcroT[i_Pcro_max][2][0];
tea5062001 0:c94b2f0a4409 1007 HcroTs[2][1] = HcroT[i_Pcro_max][2][1];
tea5062001 0:c94b2f0a4409 1008
tea5062001 0:c94b2f0a4409 1009 // S^-1 select
tea5062001 0:c94b2f0a4409 1010 Scro_is[0][0] = Scro_inv[i_Pcro_max][0][0];
tea5062001 0:c94b2f0a4409 1011 Scro_is[0][1] = Scro_inv[i_Pcro_max][0][1];
tea5062001 0:c94b2f0a4409 1012 Scro_is[1][0] = Scro_inv[i_Pcro_max][1][0];
tea5062001 0:c94b2f0a4409 1013 Scro_is[1][1] = Scro_inv[i_Pcro_max][1][1];
tea5062001 0:c94b2f0a4409 1014
tea5062001 0:c94b2f0a4409 1015 // get (Z-Z_hat)
tea5062001 0:c94b2f0a4409 1016 Zcro_delta[0][0] = Zcro[0][0] - Zcro_hs[0][0];
tea5062001 0:c94b2f0a4409 1017 Zcro_delta[1][0] = Zcro[1][0] - Zcro_hs[1][0];
tea5062001 0:c94b2f0a4409 1018
tea5062001 0:c94b2f0a4409 1019 // Sigma_bar*HT = Kcro_1
tea5062001 0:c94b2f0a4409 1020 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];
tea5062001 0:c94b2f0a4409 1021 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];
tea5062001 0:c94b2f0a4409 1022 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];
tea5062001 0:c94b2f0a4409 1023 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];
tea5062001 0:c94b2f0a4409 1024 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];
tea5062001 0:c94b2f0a4409 1025 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];
tea5062001 0:c94b2f0a4409 1026
tea5062001 0:c94b2f0a4409 1027 // Sigma_bar*HT * S^-1 = Kcro_1 * S^-1 = Kcro
tea5062001 0:c94b2f0a4409 1028 Kcro[0][0] = Kcro_1[0][0]*Scro_is[0][0] + Kcro_1[0][1]*Scro_is[1][0];
tea5062001 0:c94b2f0a4409 1029 Kcro[0][1] = Kcro_1[0][0]*Scro_is[0][1] + Kcro_1[0][1]*Scro_is[1][1];
tea5062001 0:c94b2f0a4409 1030 Kcro[1][0] = Kcro_1[1][0]*Scro_is[0][0] + Kcro_1[1][1]*Scro_is[1][0];
tea5062001 0:c94b2f0a4409 1031 Kcro[1][1] = Kcro_1[1][0]*Scro_is[0][1] + Kcro_1[1][1]*Scro_is[1][1];
tea5062001 0:c94b2f0a4409 1032 Kcro[2][0] = Kcro_1[2][0]*Scro_is[0][0] + Kcro_1[2][1]*Scro_is[1][0];
tea5062001 0:c94b2f0a4409 1033 Kcro[2][1] = Kcro_1[2][0]*Scro_is[0][1] + Kcro_1[2][1]*Scro_is[1][1];
tea5062001 0:c94b2f0a4409 1034
tea5062001 0:c94b2f0a4409 1035 // K*(Z-Z_hat) = Kcro_2
tea5062001 0:c94b2f0a4409 1036 Kcro_2[0][0] = Kcro[0][0]*Zcro_delta[0][0] + Kcro[0][1]*Zcro_delta[1][0];
tea5062001 0:c94b2f0a4409 1037 Kcro_2[1][0] = Kcro[1][0]*Zcro_delta[0][0] + Kcro[1][1]*Zcro_delta[1][0];
tea5062001 0:c94b2f0a4409 1038 Kcro_2[2][0] = Kcro[2][0]*Zcro_delta[0][0] + Kcro[2][1]*Zcro_delta[1][0];
tea5062001 0:c94b2f0a4409 1039
tea5062001 0:c94b2f0a4409 1040 // K*H = Icro_1
tea5062001 0:c94b2f0a4409 1041 Icro_1[0][0] = Kcro[0][0]*Hcros[0][0] + Kcro[0][1]*Hcros[1][0];
tea5062001 0:c94b2f0a4409 1042 Icro_1[0][1] = Kcro[0][0]*Hcros[0][1] + Kcro[0][1]*Hcros[1][1];
tea5062001 0:c94b2f0a4409 1043 Icro_1[0][2] = Kcro[0][0]*Hcros[0][2] + Kcro[0][1]*Hcros[1][2];
tea5062001 0:c94b2f0a4409 1044 Icro_1[1][0] = Kcro[1][0]*Hcros[0][0] + Kcro[1][1]*Hcros[1][0];
tea5062001 0:c94b2f0a4409 1045 Icro_1[1][1] = Kcro[1][0]*Hcros[0][1] + Kcro[1][1]*Hcros[1][1];
tea5062001 0:c94b2f0a4409 1046 Icro_1[1][2] = Kcro[1][0]*Hcros[0][2] + Kcro[1][1]*Hcros[1][2];
tea5062001 0:c94b2f0a4409 1047 Icro_1[2][0] = Kcro[2][0]*Hcros[0][0] + Kcro[2][1]*Hcros[1][0];
tea5062001 0:c94b2f0a4409 1048 Icro_1[2][1] = Kcro[2][0]*Hcros[0][1] + Kcro[2][1]*Hcros[1][1];
tea5062001 0:c94b2f0a4409 1049 Icro_1[2][2] = Kcro[2][0]*Hcros[0][2] + Kcro[2][1]*Hcros[1][2];
tea5062001 0:c94b2f0a4409 1050
tea5062001 0:c94b2f0a4409 1051 // (I - K*H) = (I - Icro_1) = Icro_2
tea5062001 0:c94b2f0a4409 1052 Icro_2[0][0] = 1 - Icro_1[0][0];
tea5062001 0:c94b2f0a4409 1053 Icro_2[0][1] = 0 - Icro_1[0][1];
tea5062001 0:c94b2f0a4409 1054 Icro_2[0][2] = 0 - Icro_1[0][2];
tea5062001 0:c94b2f0a4409 1055 Icro_2[1][0] = 0 - Icro_1[1][0];
tea5062001 0:c94b2f0a4409 1056 Icro_2[1][1] = 1 - Icro_1[1][1];
tea5062001 0:c94b2f0a4409 1057 Icro_2[1][2] = 0 - Icro_1[1][2];
tea5062001 0:c94b2f0a4409 1058 Icro_2[2][0] = 0 - Icro_1[2][0];
tea5062001 0:c94b2f0a4409 1059 Icro_2[2][1] = 0 - Icro_1[2][1];
tea5062001 0:c94b2f0a4409 1060 Icro_2[2][2] = 1 - Icro_1[2][2];
tea5062001 0:c94b2f0a4409 1061
tea5062001 0:c94b2f0a4409 1062 update_state = 1;
tea5062001 0:c94b2f0a4409 1063 }
tea5062001 0:c94b2f0a4409 1064 // if(update_source == 1) end
tea5062001 0:c94b2f0a4409 1065 else if(update_source == 2) // line detection
tea5062001 0:c94b2f0a4409 1066 {
tea5062001 0:c94b2f0a4409 1067 // line angle measurement
tea5062001 0:c94b2f0a4409 1068 Zli = Theta_c;
tea5062001 0:c94b2f0a4409 1069
tea5062001 0:c94b2f0a4409 1070 // measurement prediction covariance
tea5062001 0:c94b2f0a4409 1071 Sli = Sigma_bar[2][2] + Qli;
tea5062001 0:c94b2f0a4409 1072
tea5062001 0:c94b2f0a4409 1073 // measurement likelihood (simplified method)
tea5062001 0:c94b2f0a4409 1074 Zli_hat[0] = mu_bar_deg - 90;
tea5062001 0:c94b2f0a4409 1075 Zli_hat[1] = mu_bar_deg - 180;
tea5062001 0:c94b2f0a4409 1076 Zli_hat[2] = mu_bar_deg - 270;
tea5062001 0:c94b2f0a4409 1077 Zli_hat[3] = mu_bar_deg - 360;
tea5062001 0:c94b2f0a4409 1078 if(Zli_hat[3] < -270)
tea5062001 0:c94b2f0a4409 1079 Zli_hat[3] = Zli_hat[3] + 360;
tea5062001 0:c94b2f0a4409 1080
tea5062001 0:c94b2f0a4409 1081 // measurement difference square
tea5062001 0:c94b2f0a4409 1082 Zli_diff[0] = (Zli - Zli_hat[0]) * (Zli - Zli_hat[0]);
tea5062001 0:c94b2f0a4409 1083 Zli_diff[1] = (Zli - Zli_hat[1]) * (Zli - Zli_hat[1]);
tea5062001 0:c94b2f0a4409 1084 Zli_diff[2] = (Zli - Zli_hat[2]) * (Zli - Zli_hat[2]);
tea5062001 0:c94b2f0a4409 1085 Zli_diff[3] = (Zli - Zli_hat[3]) * (Zli - Zli_hat[3]);
tea5062001 0:c94b2f0a4409 1086
tea5062001 0:c94b2f0a4409 1087 // find the minimum measurement difference square and index
tea5062001 0:c94b2f0a4409 1088 Zli_diff_min = Zli_diff[0];
tea5062001 0:c94b2f0a4409 1089 ind_diff_min = 0;
tea5062001 0:c94b2f0a4409 1090
tea5062001 0:c94b2f0a4409 1091 for(int j = 1 ; j < 4 ; j++)
tea5062001 0:c94b2f0a4409 1092 {
tea5062001 0:c94b2f0a4409 1093 if(Zli_diff[j] < Zli_diff_min)
tea5062001 0:c94b2f0a4409 1094 {
tea5062001 0:c94b2f0a4409 1095 Zli_diff_min = Zli_diff[j];
tea5062001 0:c94b2f0a4409 1096 ind_diff_min = j;
tea5062001 0:c94b2f0a4409 1097 }
tea5062001 0:c94b2f0a4409 1098 }
tea5062001 0:c94b2f0a4409 1099
tea5062001 0:c94b2f0a4409 1100 if(Zli_diff_min <= 150)
tea5062001 0:c94b2f0a4409 1101 {
tea5062001 0:c94b2f0a4409 1102 // Kli = Sigma_bar * HT * S^-1
tea5062001 0:c94b2f0a4409 1103 Kli[2][0] = Sigma_bar[2][2] / Sli;
tea5062001 0:c94b2f0a4409 1104
tea5062001 0:c94b2f0a4409 1105 update_state = 2;
tea5062001 0:c94b2f0a4409 1106 }
tea5062001 0:c94b2f0a4409 1107 else
tea5062001 0:c94b2f0a4409 1108 {
tea5062001 0:c94b2f0a4409 1109 update_state = 0;
tea5062001 0:c94b2f0a4409 1110 }
tea5062001 0:c94b2f0a4409 1111 }
tea5062001 0:c94b2f0a4409 1112 // else if(update_source == 2) end
tea5062001 0:c94b2f0a4409 1113 else
tea5062001 0:c94b2f0a4409 1114 {
tea5062001 0:c94b2f0a4409 1115 update_state = 0;
tea5062001 0:c94b2f0a4409 1116 }
tea5062001 0:c94b2f0a4409 1117 }
tea5062001 0:c94b2f0a4409 1118
tea5062001 0:c94b2f0a4409 1119 // *** Update End ********************************************************************* //
tea5062001 0:c94b2f0a4409 1120
tea5062001 0:c94b2f0a4409 1121 if(up_dis >= up_dis_r)
tea5062001 0:c94b2f0a4409 1122 {
tea5062001 0:c94b2f0a4409 1123 // update
tea5062001 0:c94b2f0a4409 1124 if(update_state == 1)
tea5062001 0:c94b2f0a4409 1125 {
tea5062001 0:c94b2f0a4409 1126 switch(select_ups)
tea5062001 0:c94b2f0a4409 1127 {
tea5062001 0:c94b2f0a4409 1128 // 0 for no update
tea5062001 0:c94b2f0a4409 1129 case 0:
tea5062001 0:c94b2f0a4409 1130 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1131 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1132 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1133
tea5062001 0:c94b2f0a4409 1134 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1135 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1136 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1137 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1138 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1139 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1140 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1141 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1142 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1143
tea5062001 0:c94b2f0a4409 1144 up_check = 0;
tea5062001 0:c94b2f0a4409 1145 break;
tea5062001 0:c94b2f0a4409 1146
tea5062001 0:c94b2f0a4409 1147 // 1 for cross update only
tea5062001 0:c94b2f0a4409 1148 case 1:
tea5062001 0:c94b2f0a4409 1149 // mu_bar + K*(Z-Z_hat) = mu
tea5062001 0:c94b2f0a4409 1150 mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 1151 mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 1152 mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
tea5062001 0:c94b2f0a4409 1153
tea5062001 0:c94b2f0a4409 1154 // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
tea5062001 0:c94b2f0a4409 1155 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];
tea5062001 0:c94b2f0a4409 1156 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];
tea5062001 0:c94b2f0a4409 1157 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];
tea5062001 0:c94b2f0a4409 1158 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];
tea5062001 0:c94b2f0a4409 1159 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];
tea5062001 0:c94b2f0a4409 1160 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];
tea5062001 0:c94b2f0a4409 1161 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];
tea5062001 0:c94b2f0a4409 1162 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];
tea5062001 0:c94b2f0a4409 1163 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];
tea5062001 0:c94b2f0a4409 1164
tea5062001 0:c94b2f0a4409 1165 up_check = 1;
tea5062001 0:c94b2f0a4409 1166 break;
tea5062001 0:c94b2f0a4409 1167
tea5062001 0:c94b2f0a4409 1168 // 2 for line update only
tea5062001 0:c94b2f0a4409 1169 case 2:
tea5062001 0:c94b2f0a4409 1170 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1171 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1172 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1173
tea5062001 0:c94b2f0a4409 1174 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1175 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1176 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1177 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1178 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1179 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1180 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1181 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1182 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1183
tea5062001 0:c94b2f0a4409 1184 up_check = 0;
tea5062001 0:c94b2f0a4409 1185 break;
tea5062001 0:c94b2f0a4409 1186
tea5062001 0:c94b2f0a4409 1187 // 3 for cross and line update together
tea5062001 0:c94b2f0a4409 1188 case 3:
tea5062001 0:c94b2f0a4409 1189 // mu_bar + K*(Z-Z_hat) = mu
tea5062001 0:c94b2f0a4409 1190 mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 1191 mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 1192 mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
tea5062001 0:c94b2f0a4409 1193
tea5062001 0:c94b2f0a4409 1194 // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
tea5062001 0:c94b2f0a4409 1195 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];
tea5062001 0:c94b2f0a4409 1196 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];
tea5062001 0:c94b2f0a4409 1197 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];
tea5062001 0:c94b2f0a4409 1198 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];
tea5062001 0:c94b2f0a4409 1199 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];
tea5062001 0:c94b2f0a4409 1200 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];
tea5062001 0:c94b2f0a4409 1201 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];
tea5062001 0:c94b2f0a4409 1202 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];
tea5062001 0:c94b2f0a4409 1203 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];
tea5062001 0:c94b2f0a4409 1204
tea5062001 0:c94b2f0a4409 1205 up_check = 1;
tea5062001 0:c94b2f0a4409 1206 break;
tea5062001 0:c94b2f0a4409 1207 }
tea5062001 0:c94b2f0a4409 1208 }
tea5062001 0:c94b2f0a4409 1209 else if(update_state == 2)
tea5062001 0:c94b2f0a4409 1210 {
tea5062001 0:c94b2f0a4409 1211 switch(select_ups)
tea5062001 0:c94b2f0a4409 1212 {
tea5062001 0:c94b2f0a4409 1213 // 0 for no update
tea5062001 0:c94b2f0a4409 1214 case 0:
tea5062001 0:c94b2f0a4409 1215 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1216 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1217 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1218
tea5062001 0:c94b2f0a4409 1219 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1220 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1221 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1222 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1223 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1224 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1225 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1226 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1227 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1228
tea5062001 0:c94b2f0a4409 1229 up_check = 0;
tea5062001 0:c94b2f0a4409 1230 break;
tea5062001 0:c94b2f0a4409 1231
tea5062001 0:c94b2f0a4409 1232 // 1 for cross update only
tea5062001 0:c94b2f0a4409 1233 case 1:
tea5062001 0:c94b2f0a4409 1234 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1235 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1236 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1237
tea5062001 0:c94b2f0a4409 1238 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1239 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1240 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1241 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1242 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1243 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1244 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1245 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1246 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1247
tea5062001 0:c94b2f0a4409 1248 up_check = 0;
tea5062001 0:c94b2f0a4409 1249 break;
tea5062001 0:c94b2f0a4409 1250
tea5062001 0:c94b2f0a4409 1251 // 2 for line update only
tea5062001 0:c94b2f0a4409 1252 case 2:
tea5062001 0:c94b2f0a4409 1253 // mu = mu_bar + K*(Z-Z_hat)
tea5062001 0:c94b2f0a4409 1254 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1255 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1256 mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
tea5062001 0:c94b2f0a4409 1257
tea5062001 0:c94b2f0a4409 1258 // Sigma = (I - K*H) * Sigma_bar
tea5062001 0:c94b2f0a4409 1259 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1260 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1261 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1262 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1263 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1264 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1265 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1266 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1267 Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1268
tea5062001 0:c94b2f0a4409 1269 up_check = 2;
tea5062001 0:c94b2f0a4409 1270 break;
tea5062001 0:c94b2f0a4409 1271
tea5062001 0:c94b2f0a4409 1272 // 3 for cross and line update together
tea5062001 0:c94b2f0a4409 1273 case 3:
tea5062001 0:c94b2f0a4409 1274 // mu = mu_bar + K*(Z-Z_hat)
tea5062001 0:c94b2f0a4409 1275 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1276 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1277 mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
tea5062001 0:c94b2f0a4409 1278
tea5062001 0:c94b2f0a4409 1279 // Sigma = (I - K*H) * Sigma_bar
tea5062001 0:c94b2f0a4409 1280 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1281 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1282 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1283 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1284 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1285 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1286 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1287 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1288 Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1289
tea5062001 0:c94b2f0a4409 1290 up_check = 2;
tea5062001 0:c94b2f0a4409 1291 break;
tea5062001 0:c94b2f0a4409 1292 }
tea5062001 0:c94b2f0a4409 1293 }
tea5062001 0:c94b2f0a4409 1294 else
tea5062001 0:c94b2f0a4409 1295 {
tea5062001 0:c94b2f0a4409 1296 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1297 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1298 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1299
tea5062001 0:c94b2f0a4409 1300 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1301 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1302 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1303 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1304 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1305 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1306 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1307 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1308 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1309
tea5062001 0:c94b2f0a4409 1310 up_check = 0;
tea5062001 0:c94b2f0a4409 1311 }
tea5062001 0:c94b2f0a4409 1312
tea5062001 0:c94b2f0a4409 1313 up_dis = 0;
tea5062001 0:c94b2f0a4409 1314 }
tea5062001 0:c94b2f0a4409 1315 else if(up_dis <= -up_dis_r)
tea5062001 0:c94b2f0a4409 1316 {
tea5062001 0:c94b2f0a4409 1317 // update
tea5062001 0:c94b2f0a4409 1318 if(update_state == 1)
tea5062001 0:c94b2f0a4409 1319 {
tea5062001 0:c94b2f0a4409 1320 switch(select_ups)
tea5062001 0:c94b2f0a4409 1321 {
tea5062001 0:c94b2f0a4409 1322 // 0 for no update
tea5062001 0:c94b2f0a4409 1323 case 0:
tea5062001 0:c94b2f0a4409 1324 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1325 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1326 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1327
tea5062001 0:c94b2f0a4409 1328 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1329 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1330 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1331 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1332 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1333 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1334 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1335 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1336 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1337
tea5062001 0:c94b2f0a4409 1338 up_check = 0;
tea5062001 0:c94b2f0a4409 1339 break;
tea5062001 0:c94b2f0a4409 1340
tea5062001 0:c94b2f0a4409 1341 // 1 for cross update only
tea5062001 0:c94b2f0a4409 1342 case 1:
tea5062001 0:c94b2f0a4409 1343 // mu_bar + K*(Z-Z_hat) = mu
tea5062001 0:c94b2f0a4409 1344 mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 1345 mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 1346 mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
tea5062001 0:c94b2f0a4409 1347
tea5062001 0:c94b2f0a4409 1348 // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
tea5062001 0:c94b2f0a4409 1349 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];
tea5062001 0:c94b2f0a4409 1350 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];
tea5062001 0:c94b2f0a4409 1351 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];
tea5062001 0:c94b2f0a4409 1352 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];
tea5062001 0:c94b2f0a4409 1353 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];
tea5062001 0:c94b2f0a4409 1354 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];
tea5062001 0:c94b2f0a4409 1355 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];
tea5062001 0:c94b2f0a4409 1356 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];
tea5062001 0:c94b2f0a4409 1357 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];
tea5062001 0:c94b2f0a4409 1358
tea5062001 0:c94b2f0a4409 1359 up_check = 1;
tea5062001 0:c94b2f0a4409 1360 break;
tea5062001 0:c94b2f0a4409 1361
tea5062001 0:c94b2f0a4409 1362 // 2 for line update only
tea5062001 0:c94b2f0a4409 1363 case 2:
tea5062001 0:c94b2f0a4409 1364 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1365 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1366 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1367
tea5062001 0:c94b2f0a4409 1368 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1369 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1370 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1371 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1372 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1373 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1374 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1375 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1376 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1377
tea5062001 0:c94b2f0a4409 1378 up_check = 0;
tea5062001 0:c94b2f0a4409 1379 break;
tea5062001 0:c94b2f0a4409 1380
tea5062001 0:c94b2f0a4409 1381 // 3 for cross and line update together
tea5062001 0:c94b2f0a4409 1382 case 3:
tea5062001 0:c94b2f0a4409 1383 // mu_bar + K*(Z-Z_hat) = mu
tea5062001 0:c94b2f0a4409 1384 mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 1385 mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 1386 mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
tea5062001 0:c94b2f0a4409 1387
tea5062001 0:c94b2f0a4409 1388 // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
tea5062001 0:c94b2f0a4409 1389 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];
tea5062001 0:c94b2f0a4409 1390 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];
tea5062001 0:c94b2f0a4409 1391 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];
tea5062001 0:c94b2f0a4409 1392 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];
tea5062001 0:c94b2f0a4409 1393 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];
tea5062001 0:c94b2f0a4409 1394 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];
tea5062001 0:c94b2f0a4409 1395 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];
tea5062001 0:c94b2f0a4409 1396 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];
tea5062001 0:c94b2f0a4409 1397 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];
tea5062001 0:c94b2f0a4409 1398
tea5062001 0:c94b2f0a4409 1399 up_check = 1;
tea5062001 0:c94b2f0a4409 1400 break;
tea5062001 0:c94b2f0a4409 1401 }
tea5062001 0:c94b2f0a4409 1402 }
tea5062001 0:c94b2f0a4409 1403 else if(update_state == 2)
tea5062001 0:c94b2f0a4409 1404 {
tea5062001 0:c94b2f0a4409 1405 switch(select_ups)
tea5062001 0:c94b2f0a4409 1406 {
tea5062001 0:c94b2f0a4409 1407 // 0 for no update
tea5062001 0:c94b2f0a4409 1408 case 0:
tea5062001 0:c94b2f0a4409 1409 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1410 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1411 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1412
tea5062001 0:c94b2f0a4409 1413 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1414 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1415 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1416 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1417 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1418 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1419 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1420 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1421 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1422
tea5062001 0:c94b2f0a4409 1423 up_check = 0;
tea5062001 0:c94b2f0a4409 1424 break;
tea5062001 0:c94b2f0a4409 1425
tea5062001 0:c94b2f0a4409 1426 // 1 for cross update only
tea5062001 0:c94b2f0a4409 1427 case 1:
tea5062001 0:c94b2f0a4409 1428 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1429 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1430 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1431
tea5062001 0:c94b2f0a4409 1432 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1433 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1434 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1435 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1436 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1437 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1438 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1439 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1440 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1441
tea5062001 0:c94b2f0a4409 1442 up_check = 0;
tea5062001 0:c94b2f0a4409 1443 break;
tea5062001 0:c94b2f0a4409 1444
tea5062001 0:c94b2f0a4409 1445 // 2 for line update only
tea5062001 0:c94b2f0a4409 1446 case 2:
tea5062001 0:c94b2f0a4409 1447 // mu = mu_bar + K*(Z-Z_hat)
tea5062001 0:c94b2f0a4409 1448 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1449 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1450 mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
tea5062001 0:c94b2f0a4409 1451
tea5062001 0:c94b2f0a4409 1452 // Sigma = (I - K*H) * Sigma_bar
tea5062001 0:c94b2f0a4409 1453 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1454 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1455 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1456 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1457 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1458 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1459 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1460 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1461 Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1462
tea5062001 0:c94b2f0a4409 1463 up_check = 2;
tea5062001 0:c94b2f0a4409 1464 break;
tea5062001 0:c94b2f0a4409 1465
tea5062001 0:c94b2f0a4409 1466 // 3 for cross and line update together
tea5062001 0:c94b2f0a4409 1467 case 3:
tea5062001 0:c94b2f0a4409 1468 // mu = mu_bar + K*(Z-Z_hat)
tea5062001 0:c94b2f0a4409 1469 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1470 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1471 mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
tea5062001 0:c94b2f0a4409 1472
tea5062001 0:c94b2f0a4409 1473 // Sigma = (I - K*H) * Sigma_bar
tea5062001 0:c94b2f0a4409 1474 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1475 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1476 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1477 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1478 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1479 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1480 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1481 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1482 Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1483
tea5062001 0:c94b2f0a4409 1484 up_check = 2;
tea5062001 0:c94b2f0a4409 1485 break;
tea5062001 0:c94b2f0a4409 1486 }
tea5062001 0:c94b2f0a4409 1487 }
tea5062001 0:c94b2f0a4409 1488 else
tea5062001 0:c94b2f0a4409 1489 {
tea5062001 0:c94b2f0a4409 1490 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1491 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1492 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1493
tea5062001 0:c94b2f0a4409 1494 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1495 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1496 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1497 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1498 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1499 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1500 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1501 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1502 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1503
tea5062001 0:c94b2f0a4409 1504 up_check = 0;
tea5062001 0:c94b2f0a4409 1505 }
tea5062001 0:c94b2f0a4409 1506
tea5062001 0:c94b2f0a4409 1507 up_dis = 0;
tea5062001 0:c94b2f0a4409 1508 }
tea5062001 0:c94b2f0a4409 1509 else if(up_rot >= up_rot_r)
tea5062001 0:c94b2f0a4409 1510 {
tea5062001 0:c94b2f0a4409 1511 // update
tea5062001 0:c94b2f0a4409 1512 if(update_state == 1)
tea5062001 0:c94b2f0a4409 1513 {
tea5062001 0:c94b2f0a4409 1514 switch(select_ups)
tea5062001 0:c94b2f0a4409 1515 {
tea5062001 0:c94b2f0a4409 1516 // 0 for no update
tea5062001 0:c94b2f0a4409 1517 case 0:
tea5062001 0:c94b2f0a4409 1518 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1519 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1520 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1521
tea5062001 0:c94b2f0a4409 1522 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1523 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1524 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1525 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1526 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1527 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1528 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1529 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1530 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1531
tea5062001 0:c94b2f0a4409 1532 up_check = 0;
tea5062001 0:c94b2f0a4409 1533 break;
tea5062001 0:c94b2f0a4409 1534
tea5062001 0:c94b2f0a4409 1535 // 1 for cross update only
tea5062001 0:c94b2f0a4409 1536 case 1:
tea5062001 0:c94b2f0a4409 1537 // mu_bar + K*(Z-Z_hat) = mu
tea5062001 0:c94b2f0a4409 1538 mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 1539 mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 1540 mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
tea5062001 0:c94b2f0a4409 1541
tea5062001 0:c94b2f0a4409 1542 // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
tea5062001 0:c94b2f0a4409 1543 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];
tea5062001 0:c94b2f0a4409 1544 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];
tea5062001 0:c94b2f0a4409 1545 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];
tea5062001 0:c94b2f0a4409 1546 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];
tea5062001 0:c94b2f0a4409 1547 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];
tea5062001 0:c94b2f0a4409 1548 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];
tea5062001 0:c94b2f0a4409 1549 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];
tea5062001 0:c94b2f0a4409 1550 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];
tea5062001 0:c94b2f0a4409 1551 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];
tea5062001 0:c94b2f0a4409 1552
tea5062001 0:c94b2f0a4409 1553 up_check = 1;
tea5062001 0:c94b2f0a4409 1554 break;
tea5062001 0:c94b2f0a4409 1555
tea5062001 0:c94b2f0a4409 1556 // 2 for line update only
tea5062001 0:c94b2f0a4409 1557 case 2:
tea5062001 0:c94b2f0a4409 1558 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1559 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1560 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1561
tea5062001 0:c94b2f0a4409 1562 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1563 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1564 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1565 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1566 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1567 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1568 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1569 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1570 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1571
tea5062001 0:c94b2f0a4409 1572 up_check = 0;
tea5062001 0:c94b2f0a4409 1573 break;
tea5062001 0:c94b2f0a4409 1574
tea5062001 0:c94b2f0a4409 1575 // 3 for cross and line update together
tea5062001 0:c94b2f0a4409 1576 case 3:
tea5062001 0:c94b2f0a4409 1577 // mu_bar + K*(Z-Z_hat) = mu
tea5062001 0:c94b2f0a4409 1578 mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 1579 mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 1580 mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
tea5062001 0:c94b2f0a4409 1581
tea5062001 0:c94b2f0a4409 1582 // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
tea5062001 0:c94b2f0a4409 1583 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];
tea5062001 0:c94b2f0a4409 1584 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];
tea5062001 0:c94b2f0a4409 1585 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];
tea5062001 0:c94b2f0a4409 1586 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];
tea5062001 0:c94b2f0a4409 1587 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];
tea5062001 0:c94b2f0a4409 1588 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];
tea5062001 0:c94b2f0a4409 1589 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];
tea5062001 0:c94b2f0a4409 1590 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];
tea5062001 0:c94b2f0a4409 1591 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];
tea5062001 0:c94b2f0a4409 1592
tea5062001 0:c94b2f0a4409 1593 up_check = 1;
tea5062001 0:c94b2f0a4409 1594 break;
tea5062001 0:c94b2f0a4409 1595 }
tea5062001 0:c94b2f0a4409 1596 }
tea5062001 0:c94b2f0a4409 1597 else if(update_state == 2)
tea5062001 0:c94b2f0a4409 1598 {
tea5062001 0:c94b2f0a4409 1599 switch(select_ups)
tea5062001 0:c94b2f0a4409 1600 {
tea5062001 0:c94b2f0a4409 1601 // 0 for no update
tea5062001 0:c94b2f0a4409 1602 case 0:
tea5062001 0:c94b2f0a4409 1603 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1604 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1605 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1606
tea5062001 0:c94b2f0a4409 1607 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1608 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1609 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1610 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1611 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1612 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1613 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1614 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1615 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1616
tea5062001 0:c94b2f0a4409 1617 up_check = 0;
tea5062001 0:c94b2f0a4409 1618 break;
tea5062001 0:c94b2f0a4409 1619
tea5062001 0:c94b2f0a4409 1620 // 1 for cross update only
tea5062001 0:c94b2f0a4409 1621 case 1:
tea5062001 0:c94b2f0a4409 1622 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1623 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1624 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1625
tea5062001 0:c94b2f0a4409 1626 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1627 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1628 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1629 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1630 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1631 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1632 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1633 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1634 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1635
tea5062001 0:c94b2f0a4409 1636 up_check = 0;
tea5062001 0:c94b2f0a4409 1637 break;
tea5062001 0:c94b2f0a4409 1638
tea5062001 0:c94b2f0a4409 1639 // 2 for line update only
tea5062001 0:c94b2f0a4409 1640 case 2:
tea5062001 0:c94b2f0a4409 1641 // mu = mu_bar + K*(Z-Z_hat)
tea5062001 0:c94b2f0a4409 1642 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1643 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1644 mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
tea5062001 0:c94b2f0a4409 1645
tea5062001 0:c94b2f0a4409 1646 // Sigma = (I - K*H) * Sigma_bar
tea5062001 0:c94b2f0a4409 1647 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1648 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1649 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1650 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1651 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1652 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1653 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1654 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1655 Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1656
tea5062001 0:c94b2f0a4409 1657 up_check = 2;
tea5062001 0:c94b2f0a4409 1658 break;
tea5062001 0:c94b2f0a4409 1659
tea5062001 0:c94b2f0a4409 1660 // 3 for cross and line update together
tea5062001 0:c94b2f0a4409 1661 case 3:
tea5062001 0:c94b2f0a4409 1662 // mu = mu_bar + K*(Z-Z_hat)
tea5062001 0:c94b2f0a4409 1663 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1664 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1665 mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
tea5062001 0:c94b2f0a4409 1666
tea5062001 0:c94b2f0a4409 1667 // Sigma = (I - K*H) * Sigma_bar
tea5062001 0:c94b2f0a4409 1668 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1669 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1670 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1671 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1672 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1673 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1674 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1675 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1676 Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1677
tea5062001 0:c94b2f0a4409 1678 up_check = 2;
tea5062001 0:c94b2f0a4409 1679 break;
tea5062001 0:c94b2f0a4409 1680 }
tea5062001 0:c94b2f0a4409 1681 }
tea5062001 0:c94b2f0a4409 1682 else
tea5062001 0:c94b2f0a4409 1683 {
tea5062001 0:c94b2f0a4409 1684 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1685 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1686 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1687
tea5062001 0:c94b2f0a4409 1688 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1689 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1690 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1691 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1692 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1693 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1694 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1695 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1696 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1697
tea5062001 0:c94b2f0a4409 1698 up_check = 0;
tea5062001 0:c94b2f0a4409 1699 }
tea5062001 0:c94b2f0a4409 1700
tea5062001 0:c94b2f0a4409 1701 up_rot = 0;
tea5062001 0:c94b2f0a4409 1702 }
tea5062001 0:c94b2f0a4409 1703 else if(up_rot <= -up_rot_r)
tea5062001 0:c94b2f0a4409 1704 {
tea5062001 0:c94b2f0a4409 1705 // update
tea5062001 0:c94b2f0a4409 1706 if(update_state == 1)
tea5062001 0:c94b2f0a4409 1707 {
tea5062001 0:c94b2f0a4409 1708 switch(select_ups)
tea5062001 0:c94b2f0a4409 1709 {
tea5062001 0:c94b2f0a4409 1710 // 0 for no update
tea5062001 0:c94b2f0a4409 1711 case 0:
tea5062001 0:c94b2f0a4409 1712 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1713 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1714 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1715
tea5062001 0:c94b2f0a4409 1716 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1717 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1718 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1719 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1720 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1721 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1722 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1723 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1724 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1725
tea5062001 0:c94b2f0a4409 1726 up_check = 0;
tea5062001 0:c94b2f0a4409 1727 break;
tea5062001 0:c94b2f0a4409 1728
tea5062001 0:c94b2f0a4409 1729 // 1 for cross update only
tea5062001 0:c94b2f0a4409 1730 case 1:
tea5062001 0:c94b2f0a4409 1731 // mu_bar + K*(Z-Z_hat) = mu
tea5062001 0:c94b2f0a4409 1732 mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 1733 mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 1734 mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
tea5062001 0:c94b2f0a4409 1735
tea5062001 0:c94b2f0a4409 1736 // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
tea5062001 0:c94b2f0a4409 1737 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];
tea5062001 0:c94b2f0a4409 1738 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];
tea5062001 0:c94b2f0a4409 1739 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];
tea5062001 0:c94b2f0a4409 1740 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];
tea5062001 0:c94b2f0a4409 1741 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];
tea5062001 0:c94b2f0a4409 1742 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];
tea5062001 0:c94b2f0a4409 1743 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];
tea5062001 0:c94b2f0a4409 1744 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];
tea5062001 0:c94b2f0a4409 1745 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];
tea5062001 0:c94b2f0a4409 1746
tea5062001 0:c94b2f0a4409 1747 up_check = 1;
tea5062001 0:c94b2f0a4409 1748 break;
tea5062001 0:c94b2f0a4409 1749
tea5062001 0:c94b2f0a4409 1750 // 2 for line update only
tea5062001 0:c94b2f0a4409 1751 case 2:
tea5062001 0:c94b2f0a4409 1752 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1753 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1754 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1755
tea5062001 0:c94b2f0a4409 1756 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1757 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1758 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1759 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1760 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1761 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1762 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1763 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1764 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1765
tea5062001 0:c94b2f0a4409 1766 up_check = 0;
tea5062001 0:c94b2f0a4409 1767 break;
tea5062001 0:c94b2f0a4409 1768
tea5062001 0:c94b2f0a4409 1769 // 3 for cross and line update together
tea5062001 0:c94b2f0a4409 1770 case 3:
tea5062001 0:c94b2f0a4409 1771 // mu_bar + K*(Z-Z_hat) = mu
tea5062001 0:c94b2f0a4409 1772 mu_bar[0][0] = mu_bar[0][0] + Kcro_2[0][0];
tea5062001 0:c94b2f0a4409 1773 mu_bar[1][0] = mu_bar[1][0] + Kcro_2[1][0];
tea5062001 0:c94b2f0a4409 1774 mu_bar[2][0] = mu_bar[2][0] + Kcro_2[2][0];
tea5062001 0:c94b2f0a4409 1775
tea5062001 0:c94b2f0a4409 1776 // (I - K*H) * Sigma_bar = Icro_2 * Sigma_bar = Sigma
tea5062001 0:c94b2f0a4409 1777 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];
tea5062001 0:c94b2f0a4409 1778 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];
tea5062001 0:c94b2f0a4409 1779 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];
tea5062001 0:c94b2f0a4409 1780 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];
tea5062001 0:c94b2f0a4409 1781 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];
tea5062001 0:c94b2f0a4409 1782 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];
tea5062001 0:c94b2f0a4409 1783 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];
tea5062001 0:c94b2f0a4409 1784 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];
tea5062001 0:c94b2f0a4409 1785 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];
tea5062001 0:c94b2f0a4409 1786
tea5062001 0:c94b2f0a4409 1787 up_check = 1;
tea5062001 0:c94b2f0a4409 1788 break;
tea5062001 0:c94b2f0a4409 1789 }
tea5062001 0:c94b2f0a4409 1790 }
tea5062001 0:c94b2f0a4409 1791 else if(update_state == 2)
tea5062001 0:c94b2f0a4409 1792 {
tea5062001 0:c94b2f0a4409 1793 switch(select_ups)
tea5062001 0:c94b2f0a4409 1794 {
tea5062001 0:c94b2f0a4409 1795 // 0 for no update
tea5062001 0:c94b2f0a4409 1796 case 0:
tea5062001 0:c94b2f0a4409 1797 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1798 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1799 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1800
tea5062001 0:c94b2f0a4409 1801 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1802 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1803 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1804 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1805 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1806 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1807 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1808 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1809 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1810
tea5062001 0:c94b2f0a4409 1811 up_check = 0;
tea5062001 0:c94b2f0a4409 1812 break;
tea5062001 0:c94b2f0a4409 1813
tea5062001 0:c94b2f0a4409 1814 // 1 for cross update only
tea5062001 0:c94b2f0a4409 1815 case 1:
tea5062001 0:c94b2f0a4409 1816 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1817 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1818 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1819
tea5062001 0:c94b2f0a4409 1820 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1821 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1822 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1823 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1824 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1825 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1826 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1827 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1828 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1829
tea5062001 0:c94b2f0a4409 1830 up_check = 0;
tea5062001 0:c94b2f0a4409 1831 break;
tea5062001 0:c94b2f0a4409 1832
tea5062001 0:c94b2f0a4409 1833 // 2 for line update only
tea5062001 0:c94b2f0a4409 1834 case 2:
tea5062001 0:c94b2f0a4409 1835 // mu = mu_bar + K*(Z-Z_hat)
tea5062001 0:c94b2f0a4409 1836 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1837 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1838 mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
tea5062001 0:c94b2f0a4409 1839
tea5062001 0:c94b2f0a4409 1840 // Sigma = (I - K*H) * Sigma_bar
tea5062001 0:c94b2f0a4409 1841 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1842 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1843 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1844 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1845 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1846 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1847 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1848 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1849 Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1850
tea5062001 0:c94b2f0a4409 1851 up_check = 2;
tea5062001 0:c94b2f0a4409 1852 break;
tea5062001 0:c94b2f0a4409 1853
tea5062001 0:c94b2f0a4409 1854 // 3 for cross and line update together
tea5062001 0:c94b2f0a4409 1855 case 3:
tea5062001 0:c94b2f0a4409 1856 // mu = mu_bar + K*(Z-Z_hat)
tea5062001 0:c94b2f0a4409 1857 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1858 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1859 mu_bar[2][0] = mu_bar[2][0] + Kli[2][0]*(Zli - Zli_hat[ind_diff_min])*PI/180;
tea5062001 0:c94b2f0a4409 1860
tea5062001 0:c94b2f0a4409 1861 // Sigma = (I - K*H) * Sigma_bar
tea5062001 0:c94b2f0a4409 1862 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1863 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1864 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1865 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1866 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1867 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1868 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1869 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1870 Sigma_bar[2][2] = Sigma_bar[2][2] - Kli[2][0]*Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1871
tea5062001 0:c94b2f0a4409 1872 up_check = 2;
tea5062001 0:c94b2f0a4409 1873 break;
tea5062001 0:c94b2f0a4409 1874 }
tea5062001 0:c94b2f0a4409 1875 }
tea5062001 0:c94b2f0a4409 1876 else
tea5062001 0:c94b2f0a4409 1877 {
tea5062001 0:c94b2f0a4409 1878 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1879 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1880 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1881
tea5062001 0:c94b2f0a4409 1882 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1883 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1884 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1885 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1886 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1887 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1888 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1889 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1890 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1891
tea5062001 0:c94b2f0a4409 1892 up_check = 0;
tea5062001 0:c94b2f0a4409 1893 }
tea5062001 0:c94b2f0a4409 1894
tea5062001 0:c94b2f0a4409 1895 up_rot = 0;
tea5062001 0:c94b2f0a4409 1896 }
tea5062001 0:c94b2f0a4409 1897 else
tea5062001 0:c94b2f0a4409 1898 {
tea5062001 0:c94b2f0a4409 1899 mu_bar[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1900 mu_bar[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1901 mu_bar[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1902
tea5062001 0:c94b2f0a4409 1903 Sigma_bar[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1904 Sigma_bar[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1905 Sigma_bar[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1906 Sigma_bar[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1907 Sigma_bar[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1908 Sigma_bar[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1909 Sigma_bar[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1910 Sigma_bar[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1911 Sigma_bar[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1912
tea5062001 0:c94b2f0a4409 1913 up_check = 0;
tea5062001 0:c94b2f0a4409 1914 }
tea5062001 0:c94b2f0a4409 1915
tea5062001 0:c94b2f0a4409 1916 mu[0][0] = mu_bar[0][0];
tea5062001 0:c94b2f0a4409 1917 mu[1][0] = mu_bar[1][0];
tea5062001 0:c94b2f0a4409 1918 mu[2][0] = mu_bar[2][0];
tea5062001 0:c94b2f0a4409 1919
tea5062001 0:c94b2f0a4409 1920 Sigma[0][0] = Sigma_bar[0][0];
tea5062001 0:c94b2f0a4409 1921 Sigma[0][1] = Sigma_bar[0][1];
tea5062001 0:c94b2f0a4409 1922 Sigma[0][2] = Sigma_bar[0][2];
tea5062001 0:c94b2f0a4409 1923 Sigma[1][0] = Sigma_bar[1][0];
tea5062001 0:c94b2f0a4409 1924 Sigma[1][1] = Sigma_bar[1][1];
tea5062001 0:c94b2f0a4409 1925 Sigma[1][2] = Sigma_bar[1][2];
tea5062001 0:c94b2f0a4409 1926 Sigma[2][0] = Sigma_bar[2][0];
tea5062001 0:c94b2f0a4409 1927 Sigma[2][1] = Sigma_bar[2][1];
tea5062001 0:c94b2f0a4409 1928 Sigma[2][2] = Sigma_bar[2][2];
tea5062001 0:c94b2f0a4409 1929 }
tea5062001 0:c94b2f0a4409 1930 // if(fusion_flag == 1) end
tea5062001 0:c94b2f0a4409 1931 }
tea5062001 0:c94b2f0a4409 1932
tea5062001 0:c94b2f0a4409 1933 ////////// timer 5 : extended kalman filter ////////////////////////////////////////////////////////////////////////////////////
tea5062001 0:c94b2f0a4409 1934
tea5062001 0:c94b2f0a4409 1935 // timer 6 : navigation
tea5062001 0:c94b2f0a4409 1936 if((t.read_us() - lastMilli6) >= LOOPTIME6) // 100000 us = 0.1 s
tea5062001 0:c94b2f0a4409 1937 {
tea5062001 0:c94b2f0a4409 1938 lastMilli6 = t.read_us();
tea5062001 0:c94b2f0a4409 1939
tea5062001 0:c94b2f0a4409 1940 // range 0~359 deg
tea5062001 0:c94b2f0a4409 1941 mu_th_deg = mu[2][0] * 180/PI;
tea5062001 0:c94b2f0a4409 1942
tea5062001 0:c94b2f0a4409 1943 if(mu_th_deg < 0)
tea5062001 0:c94b2f0a4409 1944 mu_th_deg = mu_th_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 1945 else
tea5062001 0:c94b2f0a4409 1946 mu_th_deg = mu_th_deg % 360;
tea5062001 0:c94b2f0a4409 1947
tea5062001 0:c94b2f0a4409 1948 // range -179~180 deg
tea5062001 0:c94b2f0a4409 1949 if(mu_th_deg > 180)
tea5062001 0:c94b2f0a4409 1950 mu_th_deg_180 = mu_th_deg - 360;
tea5062001 0:c94b2f0a4409 1951 else
tea5062001 0:c94b2f0a4409 1952 mu_th_deg_180 = mu_th_deg;
tea5062001 0:c94b2f0a4409 1953
tea5062001 0:c94b2f0a4409 1954 mu_th_PI = (float)mu_th_deg_180 * PI/180;
tea5062001 0:c94b2f0a4409 1955
tea5062001 0:c94b2f0a4409 1956 int turn_point = 33;
tea5062001 0:c94b2f0a4409 1957 int end_point = turn_point + 22;
tea5062001 0:c94b2f0a4409 1958
tea5062001 0:c94b2f0a4409 1959 if(nav_state == 1)
tea5062001 0:c94b2f0a4409 1960 {
tea5062001 0:c94b2f0a4409 1961 Pxd = turn_point*1*block_length;
tea5062001 0:c94b2f0a4409 1962 Pyd = 0*1*block_length;
tea5062001 0:c94b2f0a4409 1963 Od = 0;
tea5062001 0:c94b2f0a4409 1964 }
tea5062001 0:c94b2f0a4409 1965 else if(nav_state == 2)
tea5062001 0:c94b2f0a4409 1966 {
tea5062001 0:c94b2f0a4409 1967 Pxd = turn_point*1*block_length;
tea5062001 0:c94b2f0a4409 1968 Pyd = 0*1*block_length;
tea5062001 0:c94b2f0a4409 1969 Od = PI/2;
tea5062001 0:c94b2f0a4409 1970 }
tea5062001 0:c94b2f0a4409 1971 else if(nav_state == 3)
tea5062001 0:c94b2f0a4409 1972 {
tea5062001 0:c94b2f0a4409 1973 Pxd = turn_point*1*block_length;
tea5062001 0:c94b2f0a4409 1974 Pyd = (1)*(end_point - turn_point)*1*block_length;
tea5062001 0:c94b2f0a4409 1975 Od = PI/2;
tea5062001 0:c94b2f0a4409 1976 }
tea5062001 0:c94b2f0a4409 1977 else if(nav_state == 4)
tea5062001 0:c94b2f0a4409 1978 {
tea5062001 0:c94b2f0a4409 1979 Pxd = turn_point*1*block_length;
tea5062001 0:c94b2f0a4409 1980 Pyd = (1)*(end_point - turn_point)*1*block_length;
tea5062001 0:c94b2f0a4409 1981 Od = PI;
tea5062001 0:c94b2f0a4409 1982 }
tea5062001 0:c94b2f0a4409 1983
tea5062001 0:c94b2f0a4409 1984 delta_Pxd = mu[0][0] - Pxd;
tea5062001 0:c94b2f0a4409 1985 delta_Pyd = mu[1][0] - Pyd;
tea5062001 0:c94b2f0a4409 1986 delta_Od = mu_th_PI - Od;
tea5062001 0:c94b2f0a4409 1987
tea5062001 0:c94b2f0a4409 1988 distance_error_P = sqrt(pow(delta_Pxd,2) + pow(delta_Pyd,2));
tea5062001 0:c94b2f0a4409 1989 error_O = delta_Od;
tea5062001 0:c94b2f0a4409 1990
tea5062001 0:c94b2f0a4409 1991 //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!//
tea5062001 0:c94b2f0a4409 1992
tea5062001 0:c94b2f0a4409 1993 if((Theta_c >= 70 && Theta_c <= 89) && (Theta_c >= -89 && Theta_c <= -70) && nav_flag == 1 && update_source == 2 && update_source == 1)
tea5062001 0:c94b2f0a4409 1994 {
tea5062001 0:c94b2f0a4409 1995 line_tracker = 1;
tea5062001 0:c94b2f0a4409 1996 }
tea5062001 0:c94b2f0a4409 1997 else
tea5062001 0:c94b2f0a4409 1998 {
tea5062001 0:c94b2f0a4409 1999 line_tracker = 0;
tea5062001 0:c94b2f0a4409 2000 }
tea5062001 0:c94b2f0a4409 2001
tea5062001 0:c94b2f0a4409 2002 if(nav_state == 2)
tea5062001 0:c94b2f0a4409 2003 {
tea5062001 0:c94b2f0a4409 2004 line_tracker = 0;
tea5062001 0:c94b2f0a4409 2005 }
tea5062001 0:c94b2f0a4409 2006
tea5062001 0:c94b2f0a4409 2007 if(line_tracker == 1)
tea5062001 0:c94b2f0a4409 2008 {
tea5062001 0:c94b2f0a4409 2009 float K_line_tracker = 0.02;
tea5062001 0:c94b2f0a4409 2010
tea5062001 0:c94b2f0a4409 2011 if(Theta_c >= 60) {gain_w = -K_line_tracker*(Theta_c - 90);}
tea5062001 0:c94b2f0a4409 2012 else if(Theta_c <= -60) {gain_w = -K_line_tracker*(90 + Theta_c);}
tea5062001 0:c94b2f0a4409 2013
tea5062001 0:c94b2f0a4409 2014 if(distance_error_P >= 0.39)
tea5062001 0:c94b2f0a4409 2015 {
tea5062001 0:c94b2f0a4409 2016 gain_v = 0.15;
tea5062001 0:c94b2f0a4409 2017 }
tea5062001 0:c94b2f0a4409 2018 else
tea5062001 0:c94b2f0a4409 2019 {
tea5062001 0:c94b2f0a4409 2020 gain_v = -Kv*(delta_Pxd*cos(mu_th_PI) + delta_Pyd*sin(mu_th_PI));
tea5062001 0:c94b2f0a4409 2021 }
tea5062001 0:c94b2f0a4409 2022
tea5062001 0:c94b2f0a4409 2023 vel_msg.linear.x = gain_v;
tea5062001 0:c94b2f0a4409 2024 vel_msg.angular.z = gain_w;
tea5062001 0:c94b2f0a4409 2025 p.publish(&vel_msg);
tea5062001 0:c94b2f0a4409 2026
tea5062001 0:c94b2f0a4409 2027 gain_v = -Kv*(delta_Pxd*cos(mu_th_PI) + delta_Pyd*sin(mu_th_PI));
tea5062001 0:c94b2f0a4409 2028 gain_w = -Kw*delta_Od;
tea5062001 0:c94b2f0a4409 2029
tea5062001 0:c94b2f0a4409 2030 if(gain_v >= 0)
tea5062001 0:c94b2f0a4409 2031 error_P = gain_v;
tea5062001 0:c94b2f0a4409 2032 else
tea5062001 0:c94b2f0a4409 2033 error_P = -gain_v;
tea5062001 0:c94b2f0a4409 2034
tea5062001 0:c94b2f0a4409 2035 if(gain_w >= 0)
tea5062001 0:c94b2f0a4409 2036 error_O = gain_w;
tea5062001 0:c94b2f0a4409 2037 else
tea5062001 0:c94b2f0a4409 2038 error_O = -gain_w;
tea5062001 0:c94b2f0a4409 2039 }
tea5062001 0:c94b2f0a4409 2040 else
tea5062001 0:c94b2f0a4409 2041 {
tea5062001 0:c94b2f0a4409 2042 if(distance_error_P >= 0.39)
tea5062001 0:c94b2f0a4409 2043 {
tea5062001 0:c94b2f0a4409 2044 gain_v = 0.15;
tea5062001 0:c94b2f0a4409 2045 gain_w = -Kw*delta_Od;
tea5062001 0:c94b2f0a4409 2046 }
tea5062001 0:c94b2f0a4409 2047 else
tea5062001 0:c94b2f0a4409 2048 {
tea5062001 0:c94b2f0a4409 2049 gain_v = -Kv*(delta_Pxd*cos(mu_th_PI) + delta_Pyd*sin(mu_th_PI));
tea5062001 0:c94b2f0a4409 2050 gain_w = -Kw*delta_Od;
tea5062001 0:c94b2f0a4409 2051 }
tea5062001 0:c94b2f0a4409 2052
tea5062001 0:c94b2f0a4409 2053 if(gain_v >= 0)
tea5062001 0:c94b2f0a4409 2054 error_P = gain_v;
tea5062001 0:c94b2f0a4409 2055 else
tea5062001 0:c94b2f0a4409 2056 error_P = -gain_v;
tea5062001 0:c94b2f0a4409 2057
tea5062001 0:c94b2f0a4409 2058 if(gain_w >= 0)
tea5062001 0:c94b2f0a4409 2059 error_O = gain_w;
tea5062001 0:c94b2f0a4409 2060 else
tea5062001 0:c94b2f0a4409 2061 error_O = -gain_w;
tea5062001 0:c94b2f0a4409 2062
tea5062001 0:c94b2f0a4409 2063 if(nav_flag == 1)
tea5062001 0:c94b2f0a4409 2064 {
tea5062001 0:c94b2f0a4409 2065 vel_msg.linear.x = gain_v;
tea5062001 0:c94b2f0a4409 2066 vel_msg.angular.z = gain_w;
tea5062001 0:c94b2f0a4409 2067 p.publish(&vel_msg);
tea5062001 0:c94b2f0a4409 2068 }
tea5062001 0:c94b2f0a4409 2069
tea5062001 0:c94b2f0a4409 2070 }
tea5062001 0:c94b2f0a4409 2071
tea5062001 0:c94b2f0a4409 2072 if(error_P < 0.01 && error_O < PI/90)
tea5062001 0:c94b2f0a4409 2073 {
tea5062001 0:c94b2f0a4409 2074 vel_msg.linear.x = 0;
tea5062001 0:c94b2f0a4409 2075 vel_msg.angular.z = 0;
tea5062001 0:c94b2f0a4409 2076 p.publish(&vel_msg);
tea5062001 0:c94b2f0a4409 2077
tea5062001 0:c94b2f0a4409 2078 nav_state++;
tea5062001 0:c94b2f0a4409 2079
tea5062001 0:c94b2f0a4409 2080 if(nav_state >= 5)
tea5062001 0:c94b2f0a4409 2081 {
tea5062001 0:c94b2f0a4409 2082 nav_flag = 0;
tea5062001 0:c94b2f0a4409 2083 nav_state = 0;
tea5062001 0:c94b2f0a4409 2084 }
tea5062001 0:c94b2f0a4409 2085 }
tea5062001 0:c94b2f0a4409 2086 }
tea5062001 0:c94b2f0a4409 2087
tea5062001 0:c94b2f0a4409 2088 //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!//
tea5062001 0:c94b2f0a4409 2089
tea5062001 0:c94b2f0a4409 2090 // magnetometer correction
tea5062001 0:c94b2f0a4409 2091 if(cal_flag == 1)
tea5062001 0:c94b2f0a4409 2092 {
tea5062001 0:c94b2f0a4409 2093 magx_max = magx[0];
tea5062001 0:c94b2f0a4409 2094 magx_min = magx[0];
tea5062001 0:c94b2f0a4409 2095 magy_max = magy[0];
tea5062001 0:c94b2f0a4409 2096 magy_min = magy[0];
tea5062001 0:c94b2f0a4409 2097
tea5062001 0:c94b2f0a4409 2098 magx_max2 = magx2[0];
tea5062001 0:c94b2f0a4409 2099 magx_min2 = magx2[0];
tea5062001 0:c94b2f0a4409 2100 magy_max2 = magy2[0];
tea5062001 0:c94b2f0a4409 2101 magy_min2 = magy2[0];
tea5062001 0:c94b2f0a4409 2102
tea5062001 0:c94b2f0a4409 2103 for(int i = 1 ; i < ind ; i++)
tea5062001 0:c94b2f0a4409 2104 {
tea5062001 0:c94b2f0a4409 2105 if(magx[i] > magx_max) // find x max
tea5062001 0:c94b2f0a4409 2106 magx_max = magx[i];
tea5062001 0:c94b2f0a4409 2107 if(magx[i] < magx_min) // find x min
tea5062001 0:c94b2f0a4409 2108 magx_min = magx[i];
tea5062001 0:c94b2f0a4409 2109 if(magy[i] > magy_max) // find y max
tea5062001 0:c94b2f0a4409 2110 magy_max = magy[i];
tea5062001 0:c94b2f0a4409 2111 if(magy[i] < magy_min) // find y min
tea5062001 0:c94b2f0a4409 2112 magy_min = magy[i];
tea5062001 0:c94b2f0a4409 2113
tea5062001 0:c94b2f0a4409 2114 if(magx2[i] > magx_max2) // find x max
tea5062001 0:c94b2f0a4409 2115 magx_max2 = magx2[i];
tea5062001 0:c94b2f0a4409 2116 if(magx2[i] < magx_min2) // find x min
tea5062001 0:c94b2f0a4409 2117 magx_min2 = magx2[i];
tea5062001 0:c94b2f0a4409 2118 if(magy2[i] > magy_max2) // find y max
tea5062001 0:c94b2f0a4409 2119 magy_max2 = magy2[i];
tea5062001 0:c94b2f0a4409 2120 if(magy2[i] < magy_min2) // find y min
tea5062001 0:c94b2f0a4409 2121 magy_min2 = magy2[i];
tea5062001 0:c94b2f0a4409 2122 }
tea5062001 0:c94b2f0a4409 2123
tea5062001 0:c94b2f0a4409 2124 magx0 = (magx_max + magx_min) / 2; // center x
tea5062001 0:c94b2f0a4409 2125 magy0 = (magy_max + magy_min) / 2; // center y
tea5062001 0:c94b2f0a4409 2126
tea5062001 0:c94b2f0a4409 2127 magx02 = (magx_max2 + magx_min2) / 2; // center x
tea5062001 0:c94b2f0a4409 2128 magy02 = (magy_max2 + magy_min2) / 2; // center y
tea5062001 0:c94b2f0a4409 2129
tea5062001 0:c94b2f0a4409 2130 Magn_axis_zero[0] = magx0;
tea5062001 0:c94b2f0a4409 2131 Magn_axis_zero[1] = magy0;
tea5062001 0:c94b2f0a4409 2132
tea5062001 0:c94b2f0a4409 2133 Magn_axis_zero2[0] = magx02;
tea5062001 0:c94b2f0a4409 2134 Magn_axis_zero2[1] = magy02;
tea5062001 0:c94b2f0a4409 2135
tea5062001 0:c94b2f0a4409 2136 a = (magx_max - magx_min) / 2; // long axis
tea5062001 0:c94b2f0a4409 2137 b = (magy_max - magy_min) / 2; // short axis
tea5062001 0:c94b2f0a4409 2138
tea5062001 0:c94b2f0a4409 2139 a2 = (magx_max2 - magx_min2) / 2; // long axis
tea5062001 0:c94b2f0a4409 2140 b2 = (magy_max2 - magy_min2) / 2; // short axis
tea5062001 0:c94b2f0a4409 2141
tea5062001 0:c94b2f0a4409 2142 Bex = (Magn_axis_data_f[0] - Magn_axis_zero[0]) / a;
tea5062001 0:c94b2f0a4409 2143 Bey = (Magn_axis_data_f[1] - Magn_axis_zero[1]) / b;
tea5062001 0:c94b2f0a4409 2144
tea5062001 0:c94b2f0a4409 2145 Bex2 = (Magn_axis_data_f2[0] - Magn_axis_zero2[0]) / a2;
tea5062001 0:c94b2f0a4409 2146 Bey2 = (Magn_axis_data_f2[1] - Magn_axis_zero2[1]) / b2;
tea5062001 0:c94b2f0a4409 2147
tea5062001 0:c94b2f0a4409 2148 Be = (sqrt(Bex*Bex + Bey*Bey) + sqrt(Bex2*Bex2 + Bey2*Bey2)) / 2;
tea5062001 0:c94b2f0a4409 2149
tea5062001 0:c94b2f0a4409 2150 cal_flag = 0; // calculate finish
tea5062001 0:c94b2f0a4409 2151 th = 0;
tea5062001 0:c94b2f0a4409 2152 ind = 0;
tea5062001 0:c94b2f0a4409 2153 ind_ = 0;
tea5062001 0:c94b2f0a4409 2154 BK_ = 1;
tea5062001 0:c94b2f0a4409 2155 }
tea5062001 0:c94b2f0a4409 2156 } // while(1) end
tea5062001 0:c94b2f0a4409 2157 } // int main() end
tea5062001 0:c94b2f0a4409 2158
tea5062001 0:c94b2f0a4409 2159 // Kalman filter and observer //////////////////////////////////////////////////////////////////////////////////////////////
tea5062001 0:c94b2f0a4409 2160
tea5062001 0:c94b2f0a4409 2161 void read_sensor(void)
tea5062001 0:c94b2f0a4409 2162 {
tea5062001 0:c94b2f0a4409 2163 // read sensor raw data from LSM9DS0_SH.h
tea5062001 0:c94b2f0a4409 2164 read_IMU();
tea5062001 0:c94b2f0a4409 2165
tea5062001 0:c94b2f0a4409 2166 // sensor filter data
tea5062001 0:c94b2f0a4409 2167 // gyro z
tea5062001 0:c94b2f0a4409 2168 Gyro_axis_data_f[2] = lpf(Wz,Gyro_axis_data_f_old[2],10);
tea5062001 0:c94b2f0a4409 2169 Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2170
tea5062001 0:c94b2f0a4409 2171 // magn x , magn y , magn z
tea5062001 0:c94b2f0a4409 2172 Magn_axis_data_f[0] = lpf(Mx,Magn_axis_data_f_old[0],12);
tea5062001 0:c94b2f0a4409 2173 Magn_axis_data_f_old[0] = Magn_axis_data_f[0];
tea5062001 0:c94b2f0a4409 2174 Magn_axis_data_f[1] = lpf(My,Magn_axis_data_f_old[1],12);
tea5062001 0:c94b2f0a4409 2175 Magn_axis_data_f_old[1] = Magn_axis_data_f[1];
tea5062001 0:c94b2f0a4409 2176
tea5062001 0:c94b2f0a4409 2177 Magn_axis_data_f2[0] = lpf(M2x,Magn_axis_data_f_old2[0],12);
tea5062001 0:c94b2f0a4409 2178 Magn_axis_data_f_old2[0] = Magn_axis_data_f2[0];
tea5062001 0:c94b2f0a4409 2179 Magn_axis_data_f2[1] = lpf(M2y,Magn_axis_data_f_old2[1],12);
tea5062001 0:c94b2f0a4409 2180 Magn_axis_data_f_old2[1] = Magn_axis_data_f2[1];
tea5062001 0:c94b2f0a4409 2181
tea5062001 0:c94b2f0a4409 2182 // sensor after correction
tea5062001 0:c94b2f0a4409 2183 // gyro z
tea5062001 0:c94b2f0a4409 2184 Gyro_scale[2] = (Gyro_axis_data_f[2] - Gyro_axis_zero[2]);
tea5062001 0:c94b2f0a4409 2185 w_s = Gyro_scale[2];
tea5062001 0:c94b2f0a4409 2186
tea5062001 0:c94b2f0a4409 2187 // magn x , magn y
tea5062001 0:c94b2f0a4409 2188 Magn_scale[0] = (Magn_axis_data_f[0] - Magn_axis_zero[0]) / a;
tea5062001 0:c94b2f0a4409 2189 Magn_scale[1] = (Magn_axis_data_f[1] - Magn_axis_zero[1]) / b;
tea5062001 0:c94b2f0a4409 2190
tea5062001 0:c94b2f0a4409 2191 Magn_scale2[0] = (Magn_axis_data_f2[0] - Magn_axis_zero2[0]) / a2;
tea5062001 0:c94b2f0a4409 2192 Magn_scale2[1] = (Magn_axis_data_f2[1] - Magn_axis_zero2[1]) / b2;
tea5062001 0:c94b2f0a4409 2193
tea5062001 0:c94b2f0a4409 2194 // trust index
tea5062001 0:c94b2f0a4409 2195 Q = (Magn_scale[0]*Magn_scale2[0] + Magn_scale[1]*Magn_scale2[1]) / (Be * Be);
tea5062001 0:c94b2f0a4409 2196 Q = Q - 1;
tea5062001 0:c94b2f0a4409 2197
tea5062001 0:c94b2f0a4409 2198 // magnetometer angle
tea5062001 0:c94b2f0a4409 2199 if(Magn_scale[0]>0 && Magn_scale[1]>0)
tea5062001 0:c94b2f0a4409 2200 phi = 2*PI - atan((Magn_scale[1])/(Magn_scale[0]));
tea5062001 0:c94b2f0a4409 2201 else if(Magn_scale[0]<0 && Magn_scale[1]>0)
tea5062001 0:c94b2f0a4409 2202 phi = 2*PI - (PI - atan(-(Magn_scale[1])/(Magn_scale[0])));
tea5062001 0:c94b2f0a4409 2203 else if(Magn_scale[0]<0 && Magn_scale[1]<0)
tea5062001 0:c94b2f0a4409 2204 phi = 2*PI - (PI + atan((Magn_scale[1])/(Magn_scale[0])));
tea5062001 0:c94b2f0a4409 2205 else if(Magn_scale[0]>0 && Magn_scale[1]<0)
tea5062001 0:c94b2f0a4409 2206 phi = 2*PI - (2*PI - atan(-(Magn_scale[1])/(Magn_scale[0])));
tea5062001 0:c94b2f0a4409 2207
tea5062001 0:c94b2f0a4409 2208 if(Magn_scale2[0]>0 && Magn_scale2[1]>0)
tea5062001 0:c94b2f0a4409 2209 phi2 = 2*PI - atan((Magn_scale2[1])/(Magn_scale2[0]));
tea5062001 0:c94b2f0a4409 2210 else if(Magn_scale2[0]<0 && Magn_scale2[1]>0)
tea5062001 0:c94b2f0a4409 2211 phi2 = 2*PI - (PI - atan(-(Magn_scale2[1])/(Magn_scale2[0])));
tea5062001 0:c94b2f0a4409 2212 else if(Magn_scale2[0]<0 && Magn_scale2[1]<0)
tea5062001 0:c94b2f0a4409 2213 phi2 = 2*PI - (PI + atan((Magn_scale2[1])/(Magn_scale2[0])));
tea5062001 0:c94b2f0a4409 2214 else if(Magn_scale2[0]>0 && Magn_scale2[1]<0)
tea5062001 0:c94b2f0a4409 2215 phi2 = 2*PI - (2*PI - atan(-(Magn_scale2[1])/(Magn_scale2[0])));
tea5062001 0:c94b2f0a4409 2216
tea5062001 0:c94b2f0a4409 2217 // gyro bias average
tea5062001 0:c94b2f0a4409 2218 if(gyro_flag == 0)
tea5062001 0:c94b2f0a4409 2219 {
tea5062001 0:c94b2f0a4409 2220 gyro_count++;
tea5062001 0:c94b2f0a4409 2221
tea5062001 0:c94b2f0a4409 2222 if(gyro_count == 150)
tea5062001 0:c94b2f0a4409 2223 {
tea5062001 0:c94b2f0a4409 2224 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2225 index_count++;
tea5062001 0:c94b2f0a4409 2226 }
tea5062001 0:c94b2f0a4409 2227 else if(gyro_count == 155)
tea5062001 0:c94b2f0a4409 2228 {
tea5062001 0:c94b2f0a4409 2229 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2230 index_count++;
tea5062001 0:c94b2f0a4409 2231 }
tea5062001 0:c94b2f0a4409 2232 else if(gyro_count == 160)
tea5062001 0:c94b2f0a4409 2233 {
tea5062001 0:c94b2f0a4409 2234 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2235 index_count++;
tea5062001 0:c94b2f0a4409 2236 }
tea5062001 0:c94b2f0a4409 2237 else if(gyro_count == 165)
tea5062001 0:c94b2f0a4409 2238 {
tea5062001 0:c94b2f0a4409 2239 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2240 index_count++;
tea5062001 0:c94b2f0a4409 2241 }
tea5062001 0:c94b2f0a4409 2242 else if(gyro_count == 170)
tea5062001 0:c94b2f0a4409 2243 {
tea5062001 0:c94b2f0a4409 2244 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2245 index_count++;
tea5062001 0:c94b2f0a4409 2246 }
tea5062001 0:c94b2f0a4409 2247 else if(gyro_count == 175)
tea5062001 0:c94b2f0a4409 2248 {
tea5062001 0:c94b2f0a4409 2249 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2250 index_count++;
tea5062001 0:c94b2f0a4409 2251 }
tea5062001 0:c94b2f0a4409 2252 else if(gyro_count == 180)
tea5062001 0:c94b2f0a4409 2253 {
tea5062001 0:c94b2f0a4409 2254 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2255 index_count++;
tea5062001 0:c94b2f0a4409 2256 }
tea5062001 0:c94b2f0a4409 2257 else if(gyro_count == 185)
tea5062001 0:c94b2f0a4409 2258 {
tea5062001 0:c94b2f0a4409 2259 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2260 index_count++;
tea5062001 0:c94b2f0a4409 2261 }
tea5062001 0:c94b2f0a4409 2262 else if(gyro_count == 190)
tea5062001 0:c94b2f0a4409 2263 {
tea5062001 0:c94b2f0a4409 2264 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2265 index_count++;
tea5062001 0:c94b2f0a4409 2266 }
tea5062001 0:c94b2f0a4409 2267 else if(gyro_count >= 195)
tea5062001 0:c94b2f0a4409 2268 {
tea5062001 0:c94b2f0a4409 2269 gyro_init[index_count] = Gyro_axis_data_f[2];
tea5062001 0:c94b2f0a4409 2270 int z = 0;
tea5062001 0:c94b2f0a4409 2271 for(z=0 ; z<10 ; z++)
tea5062001 0:c94b2f0a4409 2272 gyro_sum = gyro_sum + gyro_init[z];
tea5062001 0:c94b2f0a4409 2273
tea5062001 0:c94b2f0a4409 2274 Gyro_axis_zero[2] = gyro_sum / 10;
tea5062001 0:c94b2f0a4409 2275 index_count = 0;
tea5062001 0:c94b2f0a4409 2276 gyro_count = 0;
tea5062001 0:c94b2f0a4409 2277 gyro_flag = 1;
tea5062001 0:c94b2f0a4409 2278 }
tea5062001 0:c94b2f0a4409 2279 }
tea5062001 0:c94b2f0a4409 2280
tea5062001 0:c94b2f0a4409 2281 if(gyro_flag == 1)
tea5062001 0:c94b2f0a4409 2282 {
tea5062001 0:c94b2f0a4409 2283 // w_s deadzone
tea5062001 0:c94b2f0a4409 2284 int flag_d_1 = 0;
tea5062001 0:c94b2f0a4409 2285 int flag_d_2 = 0;
tea5062001 0:c94b2f0a4409 2286 flag_d_1 = w_s <= 0.006;
tea5062001 0:c94b2f0a4409 2287 flag_d_2 = w_s >= -0.006;
tea5062001 0:c94b2f0a4409 2288
tea5062001 0:c94b2f0a4409 2289 if((flag_d_1 + flag_d_2) == 2)
tea5062001 0:c94b2f0a4409 2290 w_s_d = 0;
tea5062001 0:c94b2f0a4409 2291 else
tea5062001 0:c94b2f0a4409 2292 w_s_d = w_s;
tea5062001 0:c94b2f0a4409 2293
tea5062001 0:c94b2f0a4409 2294 // w_s_d integral
tea5062001 0:c94b2f0a4409 2295 theta_s = theta_s + T*w_s_d;
tea5062001 0:c94b2f0a4409 2296
tea5062001 0:c94b2f0a4409 2297 // theta_s_deg range 0~359 deg
tea5062001 0:c94b2f0a4409 2298 theta_s_deg = theta_s * 180/PI;
tea5062001 0:c94b2f0a4409 2299
tea5062001 0:c94b2f0a4409 2300 if(theta_s_deg < 0)
tea5062001 0:c94b2f0a4409 2301 theta_s_deg = theta_s_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 2302 else
tea5062001 0:c94b2f0a4409 2303 theta_s_deg = theta_s_deg % 360;
tea5062001 0:c94b2f0a4409 2304 }
tea5062001 0:c94b2f0a4409 2305
tea5062001 0:c94b2f0a4409 2306 // phi initialize
tea5062001 0:c94b2f0a4409 2307 if(phi_init_flag == 1)
tea5062001 0:c94b2f0a4409 2308 {
tea5062001 0:c94b2f0a4409 2309 phi_init_count++;
tea5062001 0:c94b2f0a4409 2310
tea5062001 0:c94b2f0a4409 2311 if(phi_init_count >= 200)
tea5062001 0:c94b2f0a4409 2312 {
tea5062001 0:c94b2f0a4409 2313 // take current phi data for reseting absolute orientation to zero
tea5062001 0:c94b2f0a4409 2314 phi_init = phi;
tea5062001 0:c94b2f0a4409 2315 phi_init2 = phi2;
tea5062001 0:c94b2f0a4409 2316
tea5062001 0:c94b2f0a4409 2317 // reset KF and observer data
tea5062001 0:c94b2f0a4409 2318 phi_m_a_deg = 0;
tea5062001 0:c94b2f0a4409 2319 phi_m2_a_deg = 0;
tea5062001 0:c94b2f0a4409 2320 jump_count = 0;
tea5062001 0:c94b2f0a4409 2321 jump_count2 = 0;
tea5062001 0:c94b2f0a4409 2322 phi_m_deg_1 = 0;
tea5062001 0:c94b2f0a4409 2323 phi_m2_deg_1 = 0;
tea5062001 0:c94b2f0a4409 2324 phi_m_deg_2 = 0;
tea5062001 0:c94b2f0a4409 2325 phi_m2_deg_2 = 0;
tea5062001 0:c94b2f0a4409 2326 phi_m_a_avg = 0;
tea5062001 0:c94b2f0a4409 2327 theta_s = 0;
tea5062001 0:c94b2f0a4409 2328 theta_me = 0;
tea5062001 0:c94b2f0a4409 2329 theta_fu = 0;
tea5062001 0:c94b2f0a4409 2330 theta_fu_old = 0;
tea5062001 0:c94b2f0a4409 2331 bias = 0;
tea5062001 0:c94b2f0a4409 2332 bias_old = 0;
tea5062001 0:c94b2f0a4409 2333 y = 0;
tea5062001 0:c94b2f0a4409 2334
tea5062001 0:c94b2f0a4409 2335 // encoder odometry reset
tea5062001 0:c94b2f0a4409 2336 x = 0;
tea5062001 0:c94b2f0a4409 2337 y_ = 0;
tea5062001 0:c94b2f0a4409 2338 th = 0;
tea5062001 0:c94b2f0a4409 2339 x_enc = 0;
tea5062001 0:c94b2f0a4409 2340 y_enc = 0;
tea5062001 0:c94b2f0a4409 2341 th_enc = 0;
tea5062001 0:c94b2f0a4409 2342 x_emg = 0;
tea5062001 0:c94b2f0a4409 2343 y_emg = 0;
tea5062001 0:c94b2f0a4409 2344 th_emg = 0;
tea5062001 0:c94b2f0a4409 2345
tea5062001 0:c94b2f0a4409 2346 // reset EKF data
tea5062001 0:c94b2f0a4409 2347 for(int i = 0 ; i < 3 ; i++)
tea5062001 0:c94b2f0a4409 2348 {
tea5062001 0:c94b2f0a4409 2349 mu_bar[i][0] = 0;
tea5062001 0:c94b2f0a4409 2350 mu[i][0] = 0;
tea5062001 0:c94b2f0a4409 2351
tea5062001 0:c94b2f0a4409 2352 for(int j = 0 ; j < 3 ; j++)
tea5062001 0:c94b2f0a4409 2353 {
tea5062001 0:c94b2f0a4409 2354 Sigma_bar[i][j] = 0;
tea5062001 0:c94b2f0a4409 2355 Sigma[i][j] = 0;
tea5062001 0:c94b2f0a4409 2356 }
tea5062001 0:c94b2f0a4409 2357 }
tea5062001 0:c94b2f0a4409 2358
tea5062001 0:c94b2f0a4409 2359 // stop initialization
tea5062001 0:c94b2f0a4409 2360 phi_init_count = 0;
tea5062001 0:c94b2f0a4409 2361 phi_init_flag = 0;
tea5062001 0:c94b2f0a4409 2362
tea5062001 0:c94b2f0a4409 2363 // start fusion
tea5062001 0:c94b2f0a4409 2364 fusion_flag = 1;
tea5062001 0:c94b2f0a4409 2365 B_ = 1; // dark
tea5062001 0:c94b2f0a4409 2366 }
tea5062001 0:c94b2f0a4409 2367 }
tea5062001 0:c94b2f0a4409 2368
tea5062001 0:c94b2f0a4409 2369 // phi_m_deg range 0~359 deg
tea5062001 0:c94b2f0a4409 2370 phi_m = phi - phi_init;
tea5062001 0:c94b2f0a4409 2371 phi_m_deg = phi_m * 180/PI;
tea5062001 0:c94b2f0a4409 2372
tea5062001 0:c94b2f0a4409 2373 if(phi_m_deg < 0)
tea5062001 0:c94b2f0a4409 2374 phi_m_deg = phi_m_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 2375 else
tea5062001 0:c94b2f0a4409 2376 phi_m_deg = phi_m_deg % 360;
tea5062001 0:c94b2f0a4409 2377
tea5062001 0:c94b2f0a4409 2378 phi_m2 = phi2 - phi_init2;
tea5062001 0:c94b2f0a4409 2379 phi_m2_deg = phi_m2 * 180/PI;
tea5062001 0:c94b2f0a4409 2380
tea5062001 0:c94b2f0a4409 2381 if(phi_m2_deg < 0)
tea5062001 0:c94b2f0a4409 2382 phi_m2_deg = phi_m2_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 2383 else
tea5062001 0:c94b2f0a4409 2384 phi_m2_deg = phi_m2_deg % 360;
tea5062001 0:c94b2f0a4409 2385
tea5062001 0:c94b2f0a4409 2386 // phi_m_a accumulate form
tea5062001 0:c94b2f0a4409 2387 diff_phi_count++;
tea5062001 0:c94b2f0a4409 2388
tea5062001 0:c94b2f0a4409 2389 if(diff_phi_flag == 0)
tea5062001 0:c94b2f0a4409 2390 {
tea5062001 0:c94b2f0a4409 2391 phi_m_deg_1 = phi_m_deg;
tea5062001 0:c94b2f0a4409 2392 phi_m2_deg_1 = phi_m2_deg;
tea5062001 0:c94b2f0a4409 2393 diff_phi_flag = 1;
tea5062001 0:c94b2f0a4409 2394 }
tea5062001 0:c94b2f0a4409 2395 else if(diff_phi_flag == 1)
tea5062001 0:c94b2f0a4409 2396 {
tea5062001 0:c94b2f0a4409 2397 if(diff_phi_count >= 2)
tea5062001 0:c94b2f0a4409 2398 {
tea5062001 0:c94b2f0a4409 2399 phi_m_deg_2 = phi_m_deg + 360*jump_count;
tea5062001 0:c94b2f0a4409 2400 phi_m_deg_T = phi_m_deg_2 - phi_m_deg_1;
tea5062001 0:c94b2f0a4409 2401
tea5062001 0:c94b2f0a4409 2402 if(phi_m_deg_T <= -300)
tea5062001 0:c94b2f0a4409 2403 {
tea5062001 0:c94b2f0a4409 2404 phi_m_a_deg = phi_m_deg_2 + 360;
tea5062001 0:c94b2f0a4409 2405 phi_m_deg_1 = phi_m_a_deg;
tea5062001 0:c94b2f0a4409 2406 jump_count++;
tea5062001 0:c94b2f0a4409 2407 }
tea5062001 0:c94b2f0a4409 2408 else if(phi_m_deg_T >= 300)
tea5062001 0:c94b2f0a4409 2409 {
tea5062001 0:c94b2f0a4409 2410 phi_m_a_deg = phi_m_deg_2 - 360;
tea5062001 0:c94b2f0a4409 2411 phi_m_deg_1 = phi_m_a_deg;
tea5062001 0:c94b2f0a4409 2412 jump_count--;
tea5062001 0:c94b2f0a4409 2413 }
tea5062001 0:c94b2f0a4409 2414 else
tea5062001 0:c94b2f0a4409 2415 {
tea5062001 0:c94b2f0a4409 2416 phi_m_a_deg = phi_m_deg_2;
tea5062001 0:c94b2f0a4409 2417 phi_m_deg_1 = phi_m_a_deg;
tea5062001 0:c94b2f0a4409 2418 }
tea5062001 0:c94b2f0a4409 2419
tea5062001 0:c94b2f0a4409 2420 phi_m_a = phi_m_a_deg * PI/180;
tea5062001 0:c94b2f0a4409 2421
tea5062001 0:c94b2f0a4409 2422 ////////////////////////////////////////////////////////////////////
tea5062001 0:c94b2f0a4409 2423
tea5062001 0:c94b2f0a4409 2424 phi_m2_deg_2 = phi_m2_deg + 360*jump_count2;
tea5062001 0:c94b2f0a4409 2425 phi_m2_deg_T = phi_m2_deg_2 - phi_m2_deg_1;
tea5062001 0:c94b2f0a4409 2426
tea5062001 0:c94b2f0a4409 2427 if(phi_m2_deg_T <= -300)
tea5062001 0:c94b2f0a4409 2428 {
tea5062001 0:c94b2f0a4409 2429 phi_m2_a_deg = phi_m2_deg_2 + 360;
tea5062001 0:c94b2f0a4409 2430 phi_m2_deg_1 = phi_m2_a_deg;
tea5062001 0:c94b2f0a4409 2431 jump_count2++;
tea5062001 0:c94b2f0a4409 2432 }
tea5062001 0:c94b2f0a4409 2433 else if(phi_m2_deg_T >= 300)
tea5062001 0:c94b2f0a4409 2434 {
tea5062001 0:c94b2f0a4409 2435 phi_m2_a_deg = phi_m2_deg_2 - 360;
tea5062001 0:c94b2f0a4409 2436 phi_m2_deg_1 = phi_m2_a_deg;
tea5062001 0:c94b2f0a4409 2437 jump_count2--;
tea5062001 0:c94b2f0a4409 2438 }
tea5062001 0:c94b2f0a4409 2439 else
tea5062001 0:c94b2f0a4409 2440 {
tea5062001 0:c94b2f0a4409 2441 phi_m2_a_deg = phi_m2_deg_2;
tea5062001 0:c94b2f0a4409 2442 phi_m2_deg_1 = phi_m2_a_deg;
tea5062001 0:c94b2f0a4409 2443 }
tea5062001 0:c94b2f0a4409 2444
tea5062001 0:c94b2f0a4409 2445 phi_m2_a = phi_m2_a_deg * PI/180;
tea5062001 0:c94b2f0a4409 2446
tea5062001 0:c94b2f0a4409 2447 phi_m_a_avg = 0.7*phi_m_a + 0.3*phi_m2_a;
tea5062001 0:c94b2f0a4409 2448
tea5062001 0:c94b2f0a4409 2449 diff_phi_count = 0;
tea5062001 0:c94b2f0a4409 2450 }
tea5062001 0:c94b2f0a4409 2451 }
tea5062001 0:c94b2f0a4409 2452
tea5062001 0:c94b2f0a4409 2453 // theta_me Kalman filter
tea5062001 0:c94b2f0a4409 2454 // prediction
tea5062001 0:c94b2f0a4409 2455 theta_me_bar = At*theta_me + Bt*omega_z;
tea5062001 0:c94b2f0a4409 2456 sigma_bar = At*sigma*At + Rt;
tea5062001 0:c94b2f0a4409 2457
tea5062001 0:c94b2f0a4409 2458 if(Q >= -0.009 && Q <= 0.009)
tea5062001 0:c94b2f0a4409 2459 {
tea5062001 0:c94b2f0a4409 2460 if(Q >= 0)
tea5062001 0:c94b2f0a4409 2461 V = 0.74*Q;
tea5062001 0:c94b2f0a4409 2462 else
tea5062001 0:c94b2f0a4409 2463 V = -1.13*Q;
tea5062001 0:c94b2f0a4409 2464
tea5062001 0:c94b2f0a4409 2465 update_flag = 1;
tea5062001 0:c94b2f0a4409 2466 G_ = 0;
tea5062001 0:c94b2f0a4409 2467 }
tea5062001 0:c94b2f0a4409 2468 else
tea5062001 0:c94b2f0a4409 2469 {
tea5062001 0:c94b2f0a4409 2470 update_flag = 0;
tea5062001 0:c94b2f0a4409 2471 G_ = 1;
tea5062001 0:c94b2f0a4409 2472 }
tea5062001 0:c94b2f0a4409 2473
tea5062001 0:c94b2f0a4409 2474 // update distance
tea5062001 0:c94b2f0a4409 2475 update_distance = update_distance + T*vel_x;
tea5062001 0:c94b2f0a4409 2476 update_rotation = update_rotation + T*omega_z;
tea5062001 0:c94b2f0a4409 2477
tea5062001 0:c94b2f0a4409 2478 if(update_distance >= 0.1)
tea5062001 0:c94b2f0a4409 2479 {
tea5062001 0:c94b2f0a4409 2480 // update
tea5062001 0:c94b2f0a4409 2481 if(update_flag == 1)
tea5062001 0:c94b2f0a4409 2482 {
tea5062001 0:c94b2f0a4409 2483 K = sigma_bar*Ct / (Ct*sigma_bar*Ct + V);
tea5062001 0:c94b2f0a4409 2484 theta_me = theta_me_bar + K*(phi_m_a_avg - Ct*theta_me_bar);
tea5062001 0:c94b2f0a4409 2485 sigma = (1 - K*Ct)*sigma_bar;
tea5062001 0:c94b2f0a4409 2486 }
tea5062001 0:c94b2f0a4409 2487 else
tea5062001 0:c94b2f0a4409 2488 {
tea5062001 0:c94b2f0a4409 2489 theta_me = theta_me_bar;
tea5062001 0:c94b2f0a4409 2490 sigma = sigma_bar;
tea5062001 0:c94b2f0a4409 2491 }
tea5062001 0:c94b2f0a4409 2492
tea5062001 0:c94b2f0a4409 2493 update_distance = 0;
tea5062001 0:c94b2f0a4409 2494 }
tea5062001 0:c94b2f0a4409 2495 else if(update_distance <= -0.1)
tea5062001 0:c94b2f0a4409 2496 {
tea5062001 0:c94b2f0a4409 2497 // update
tea5062001 0:c94b2f0a4409 2498 if(update_flag == 1)
tea5062001 0:c94b2f0a4409 2499 {
tea5062001 0:c94b2f0a4409 2500 K = sigma_bar*Ct / (Ct*sigma_bar*Ct + V);
tea5062001 0:c94b2f0a4409 2501 theta_me = theta_me_bar + K*(phi_m_a_avg - Ct*theta_me_bar);
tea5062001 0:c94b2f0a4409 2502 sigma = (1 - K*Ct)*sigma_bar;
tea5062001 0:c94b2f0a4409 2503 }
tea5062001 0:c94b2f0a4409 2504 else
tea5062001 0:c94b2f0a4409 2505 {
tea5062001 0:c94b2f0a4409 2506 theta_me = theta_me_bar;
tea5062001 0:c94b2f0a4409 2507 sigma = sigma_bar;
tea5062001 0:c94b2f0a4409 2508 }
tea5062001 0:c94b2f0a4409 2509
tea5062001 0:c94b2f0a4409 2510 update_distance = 0;
tea5062001 0:c94b2f0a4409 2511 }
tea5062001 0:c94b2f0a4409 2512 else if(update_rotation >= PI/8)
tea5062001 0:c94b2f0a4409 2513 {
tea5062001 0:c94b2f0a4409 2514 // update
tea5062001 0:c94b2f0a4409 2515 if(update_flag == 1)
tea5062001 0:c94b2f0a4409 2516 {
tea5062001 0:c94b2f0a4409 2517 K = sigma_bar*Ct / (Ct*sigma_bar*Ct + V);
tea5062001 0:c94b2f0a4409 2518 theta_me = theta_me_bar + K*(phi_m_a_avg - Ct*theta_me_bar);
tea5062001 0:c94b2f0a4409 2519 sigma = (1 - K*Ct)*sigma_bar;
tea5062001 0:c94b2f0a4409 2520 }
tea5062001 0:c94b2f0a4409 2521 else
tea5062001 0:c94b2f0a4409 2522 {
tea5062001 0:c94b2f0a4409 2523 theta_me = theta_me_bar;
tea5062001 0:c94b2f0a4409 2524 sigma = sigma_bar;
tea5062001 0:c94b2f0a4409 2525 }
tea5062001 0:c94b2f0a4409 2526
tea5062001 0:c94b2f0a4409 2527 update_rotation = 0;
tea5062001 0:c94b2f0a4409 2528 }
tea5062001 0:c94b2f0a4409 2529 else if(update_rotation <= -PI/8)
tea5062001 0:c94b2f0a4409 2530 {
tea5062001 0:c94b2f0a4409 2531 // update
tea5062001 0:c94b2f0a4409 2532 if(update_flag == 1)
tea5062001 0:c94b2f0a4409 2533 {
tea5062001 0:c94b2f0a4409 2534 K = sigma_bar*Ct / (Ct*sigma_bar*Ct + V);
tea5062001 0:c94b2f0a4409 2535 theta_me = theta_me_bar + K*(phi_m_a_avg - Ct*theta_me_bar);
tea5062001 0:c94b2f0a4409 2536 sigma = (1 - K*Ct)*sigma_bar;
tea5062001 0:c94b2f0a4409 2537 }
tea5062001 0:c94b2f0a4409 2538 else
tea5062001 0:c94b2f0a4409 2539 {
tea5062001 0:c94b2f0a4409 2540 theta_me = theta_me_bar;
tea5062001 0:c94b2f0a4409 2541 sigma = sigma_bar;
tea5062001 0:c94b2f0a4409 2542 }
tea5062001 0:c94b2f0a4409 2543
tea5062001 0:c94b2f0a4409 2544 update_rotation = 0;
tea5062001 0:c94b2f0a4409 2545 }
tea5062001 0:c94b2f0a4409 2546 else
tea5062001 0:c94b2f0a4409 2547 {
tea5062001 0:c94b2f0a4409 2548 theta_me = theta_me_bar;
tea5062001 0:c94b2f0a4409 2549 sigma = sigma_bar;
tea5062001 0:c94b2f0a4409 2550 }
tea5062001 0:c94b2f0a4409 2551
tea5062001 0:c94b2f0a4409 2552 // theta_me_deg range 0~359 deg
tea5062001 0:c94b2f0a4409 2553 theta_me_deg = theta_me * 180/PI;
tea5062001 0:c94b2f0a4409 2554
tea5062001 0:c94b2f0a4409 2555 if(theta_me_deg < 0)
tea5062001 0:c94b2f0a4409 2556 theta_me_deg = theta_me_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 2557 else
tea5062001 0:c94b2f0a4409 2558 theta_me_deg = theta_me_deg % 360;
tea5062001 0:c94b2f0a4409 2559
tea5062001 0:c94b2f0a4409 2560 // theta_e_deg range 0~359 deg
tea5062001 0:c94b2f0a4409 2561 th_deg = th * 180/PI;
tea5062001 0:c94b2f0a4409 2562
tea5062001 0:c94b2f0a4409 2563 if(th_deg < 0)
tea5062001 0:c94b2f0a4409 2564 th_deg = th_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 2565 else
tea5062001 0:c94b2f0a4409 2566 th_deg = th_deg % 360;
tea5062001 0:c94b2f0a4409 2567
tea5062001 0:c94b2f0a4409 2568 if(fusion_flag == 1)
tea5062001 0:c94b2f0a4409 2569 {
tea5062001 0:c94b2f0a4409 2570 // Kalman filter
tea5062001 0:c94b2f0a4409 2571 L1 = 7;
tea5062001 0:c94b2f0a4409 2572 L2 = 13;
tea5062001 0:c94b2f0a4409 2573
tea5062001 0:c94b2f0a4409 2574 y = theta_me;
tea5062001 0:c94b2f0a4409 2575
tea5062001 0:c94b2f0a4409 2576 if(bias_flag == 1)
tea5062001 0:c94b2f0a4409 2577 bias_add = bias_add - r_01*r_01*0.000001 + r_01*0.000000000001;
tea5062001 0:c94b2f0a4409 2578 else
tea5062001 0:c94b2f0a4409 2579 bias_add = 0;
tea5062001 0:c94b2f0a4409 2580
tea5062001 0:c94b2f0a4409 2581 // sensor fusion algorithm
tea5062001 0:c94b2f0a4409 2582 theta_fu = (theta_fu_old + T*(w_s_d + bias_add) + L1*T*y + T*bias) / (1 + L1*T);
tea5062001 0:c94b2f0a4409 2583 theta_fu_old = theta_fu;
tea5062001 0:c94b2f0a4409 2584
tea5062001 0:c94b2f0a4409 2585 bias = bias_old + L2*T*(y - theta_fu);
tea5062001 0:c94b2f0a4409 2586 bias_old = bias;
tea5062001 0:c94b2f0a4409 2587
tea5062001 0:c94b2f0a4409 2588 // theta_fu_deg range 0~359 deg
tea5062001 0:c94b2f0a4409 2589 theta_fu_deg = theta_fu * 180/PI;
tea5062001 0:c94b2f0a4409 2590
tea5062001 0:c94b2f0a4409 2591 if(theta_fu_deg < 0)
tea5062001 0:c94b2f0a4409 2592 theta_fu_deg = theta_fu_deg % 360 + 360;
tea5062001 0:c94b2f0a4409 2593 else
tea5062001 0:c94b2f0a4409 2594 theta_fu_deg = theta_fu_deg % 360;
tea5062001 0:c94b2f0a4409 2595
tea5062001 0:c94b2f0a4409 2596 // theta_fu_deg_180 range -179~180 deg
tea5062001 0:c94b2f0a4409 2597 if(theta_fu_deg > 180)
tea5062001 0:c94b2f0a4409 2598 theta_fu_deg_180 = theta_fu_deg - 360;
tea5062001 0:c94b2f0a4409 2599 else
tea5062001 0:c94b2f0a4409 2600 theta_fu_deg_180 = theta_fu_deg;
tea5062001 0:c94b2f0a4409 2601
tea5062001 0:c94b2f0a4409 2602 theta_fu_PI = (float)theta_fu_deg_180 * PI/180;
tea5062001 0:c94b2f0a4409 2603 } // if(fusion_flag == 1) end
tea5062001 0:c94b2f0a4409 2604 }
tea5062001 0:c94b2f0a4409 2605
tea5062001 0:c94b2f0a4409 2606 // Kalman filter and observer //////////////////////////////////////////////////////////////////////////////////////////////
tea5062001 0:c94b2f0a4409 2607
tea5062001 0:c94b2f0a4409 2608 float lpf(float input, float output_old, float frequency)
tea5062001 0:c94b2f0a4409 2609 {
tea5062001 0:c94b2f0a4409 2610 float output = 0;
tea5062001 0:c94b2f0a4409 2611
tea5062001 0:c94b2f0a4409 2612 output = (output_old + frequency*T*input) / (1 + frequency*T);
tea5062001 0:c94b2f0a4409 2613
tea5062001 0:c94b2f0a4409 2614 return output;
tea5062001 0:c94b2f0a4409 2615 }