├── 二维定位推导.pdf ├── screenshot.png ├── README.md ├── CMakeLists.txt ├── math_lib.h ├── main.cpp └── world.h /二维定位推导.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imuncle/ESKF-2D-localization/main/二维定位推导.pdf -------------------------------------------------------------------------------- /screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imuncle/ESKF-2D-localization/main/screenshot.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ESKF-2D-localization 2 | a very simple implementation for ESKF 3 | 4 | ## Dependencies 5 | * OpenCV3 6 | * Eigen3 7 | 8 | ## Compile 9 | ```bash 10 | mkdir build 11 | cd build 12 | cmake .. 13 | make 14 | ``` 15 | 16 | ## Usage 17 | ```bash 18 | ./eskf_locate 19 | ``` 20 | 21 | ![ScreenShot](screenshot.png) 22 | 23 | * use mouse to drag the car (in left part view). 24 | * long press `Q` or `E` to rotate the car. 25 | * press `Esc` to exit. 26 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(eskf_locate) 3 | 4 | add_compile_options(-std=c++14) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | 7 | find_package(OpenCV REQUIRED) 8 | find_package(Eigen3 REQUIRED) 9 | 10 | include_directories( 11 | ${OpenCV_INCLUDE_DIRS} 12 | ${EIGEN3_INCLUDE_DIRS} 13 | ) 14 | 15 | add_executable(locate 16 | main.cpp 17 | ) 18 | target_link_libraries(locate 19 | ${OpenCV_LIBRARIES} 20 | ${EIGEN3_LIBRARIES} 21 | ) 22 | -------------------------------------------------------------------------------- /math_lib.h: -------------------------------------------------------------------------------- 1 | #ifndef MATH_LIB_H 2 | #define MATH_LIB_H 3 | 4 | #include 5 | 6 | double cross_product(cv::Point2d& p1, cv::Point2d& p2) 7 | { 8 | return p1.x*p2.y-p2.x*p1.y; 9 | } 10 | 11 | double product(cv::Point2d& p1, cv::Point2d& p2) 12 | { 13 | return p1.x*p2.x + p1.y*p2.y; 14 | } 15 | 16 | cv::Point2d calc_normal(cv::Point2d dir) 17 | { 18 | double len = std::sqrt(dir.x*dir.x + dir.y*dir.y); 19 | return cv::Point2d(-dir.y/len, dir.x/len); 20 | } 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include "world.h" 2 | 3 | int map_width = 200; 4 | int map_height = 200; 5 | 6 | bool mouse_move = false; 7 | void mouse_callback(int event,int x,int y,int flags,void *ustc) 8 | { 9 | 10 | switch (event) 11 | { 12 | case cv::EVENT_LBUTTONDOWN: 13 | { 14 | mouse_move = true; 15 | break; 16 | } 17 | case cv::EVENT_MOUSEMOVE: 18 | { 19 | if(x >= map_width) break; 20 | if(mouse_move) 21 | { 22 | Car* car = (Car*)ustc; 23 | car->update_pos(x, map_height-y); 24 | } 25 | break; 26 | } 27 | case cv::EVENT_LBUTTONUP: 28 | { 29 | mouse_move = false; 30 | break; 31 | } 32 | default: 33 | break; 34 | } 35 | } 36 | 37 | int main() 38 | { 39 | // 初始化地图 40 | Map map(map_width, map_height); 41 | // 加入一辆车 42 | Car car; 43 | cv::namedWindow("viz"); 44 | cv::setMouseCallback("viz", mouse_callback, (void*)&car); 45 | cv::Mat map_img = cv::Mat(map_height, map_width*2, CV_8UC3, cv::Scalar(255,255,255)); 46 | cv::line(map_img, cv::Point(map_width,0), cv::Point(map_width, map_height), cv::Scalar(0,0,0), 1); 47 | for(int i = 0; i < map.points.size()-1; i++) 48 | { 49 | cv::Point p1 = cv::Point(map.points[i].x*100, map_height-map.points[i].y*100); 50 | cv::Point p2 = cv::Point(map.points[i+1].x*100, map_height-map.points[i+1].y*100); 51 | cv::line(map_img, p1, p2, cv::Scalar(0,255,0), 2); 52 | cv::line(map_img, p1+cv::Point(map_width, 0), p2+cv::Point(map_width, 0), cv::Scalar(0,255,0), 2); 53 | } 54 | double current_time; 55 | double last_time = cv::getTickCount(); 56 | while(true) 57 | { 58 | // Step 1. 更新IMU测量 59 | car.update_imu(); 60 | // Step 2. 更新先验协方差，积分名义状态 61 | car.update_nominal(); 62 | // Step 3. 10Hz频率更新雷达测量 63 | if(((double)cv::getTickCount() - last_time)/cv::getTickFrequency() > 0.1) 64 | { 65 | last_time = cv::getTickCount(); 66 | car.update_lidar(map); 67 | // Step 4. 测量更新 68 | car.update_estimation(map); 69 | } 70 | 71 | // 把结果可视化 72 | cv::Mat show_map = map_img.clone(); 73 | cv::Point2d p1(car.true_state.pos[0]*100, map_height-car.true_state.pos[1]*100); 74 | cv::Point2d p2 = p1 + 30*cv::Point2d(std::cos(car.true_state.theta), -std::sin(car.true_state.theta)); 75 | cv::arrowedLine(show_map, p1, p2, cv::Scalar(0,0,255), 2); 76 | cv::circle(show_map, p1, 5, cv::Scalar(255,0,0), 2); 77 | for(int i = 0; i < car.lidar.scan_point.size(); i++) 78 | { 79 | cv::Point2d p = car.lidar.scan_point[i]; 80 | cv::circle(show_map, cv::Point(p.x*100, map_height-p.y*100), 3, cv::Scalar(255,255,0), 2); 81 | p = car.lidar_points[i]; 82 | cv::circle(show_map, cv::Point(map_width+p.x*100, map_height-p.y*100), 3, cv::Scalar(255,0,255), 2); 83 | } 84 | p1 = cv::Point2d(map_width+car.nominal_state.pos[0]*100, map_height-car.nominal_state.pos[1]*100); 85 | p2 = p1 + 30*cv::Point2d(std::cos(car.nominal_state.theta), -std::sin(car.nominal_state.theta)); 86 | cv::arrowedLine(show_map, p1, p2, cv::Scalar(0,0,255), 2); 87 | cv::circle(show_map, p1, 5, cv::Scalar(255,0,0), 2); 88 | cv::imshow("viz", show_map); 89 | int k = cv::waitKey(1); 90 | if(k == 'q') 91 | car.new_theta += 0.1; 92 | else if(k == 'e') 93 | car.new_theta -= 0.1; 94 | else if(k == 27) 95 | break; 96 | } 97 | return 0; 98 | } -------------------------------------------------------------------------------- /world.h: -------------------------------------------------------------------------------- 1 | #ifndef WORLD_H 2 | #define WORLD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "math_lib.h" 8 | #include 9 | 10 | class Map 11 | { 12 | public: 13 | Map(int w, int h){ 14 | width = w; 15 | height = h; 16 | points.push_back(cv::Point2d(0,0)); 17 | points.push_back(cv::Point2d(0,1)); 18 | points.push_back(cv::Point2d(1,1)); 19 | points.push_back(cv::Point2d(1,2)); 20 | points.push_back(cv::Point2d(2,2)); 21 | points.push_back(cv::Point2d(2,0)); 22 | points.push_back(cv::Point2d(0,0)); 23 | } 24 | ~Map(){} 25 | double find_nearest(cv::Point2d p, cv::Point2d& dir) 26 | { 27 | double min_dis = 100; 28 | for(int i = 0; i < points.size()-1; i++) 29 | { 30 | cv::Point2d dir1 = points[i+1] - points[i]; 31 | dir1 = calc_normal(dir1); 32 | cv::Point2d dir2 = points[i] - p; 33 | double dis = product(dir1, dir2); 34 | if(fabs(dis) < fabs(min_dis)) 35 | { 36 | min_dis = dis; 37 | dir = dir1; 38 | } 39 | } 40 | return min_dis; 41 | } 42 | int width; 43 | int height; 44 | std::vector points; 45 | }; 46 | 47 | class Lidar 48 | { 49 | public: 50 | Lidar(){ 51 | data.resize(8); 52 | angle = std::vector{0, M_PI/4, M_PI/2, M_PI*0.75, M_PI, M_PI*1.25, M_PI*1.5, M_PI*1.75}; 53 | scan_point.resize(8); 54 | scan_dir.push_back(cv::Point2d(1,0)); 55 | scan_dir.push_back(cv::Point2d(1,1)); 56 | scan_dir.push_back(cv::Point2d(0,1)); 57 | scan_dir.push_back(cv::Point2d(-1,1)); 58 | scan_dir.push_back(cv::Point2d(-1,0)); 59 | scan_dir.push_back(cv::Point2d(-1,-1)); 60 | scan_dir.push_back(cv::Point2d(0,-1)); 61 | scan_dir.push_back(cv::Point2d(1,-1)); 62 | } 63 | ~Lidar(){} 64 | void measure(double* position, double theta, const Map& map) 65 | { 66 | cv::Point2d pos(position[0], position[1]); 67 | // 雷达按【米】字形扫八个点 68 | for(int i = 0; i < scan_dir.size(); i++) 69 | { 70 | // 首先计算世界坐标系下的扫描方向 71 | cv::Point2d dir; 72 | dir.x = std::cos(theta)*scan_dir[i].x - std::sin(theta)*scan_dir[i].y; 73 | dir.y = std::sin(theta)*scan_dir[i].x + std::cos(theta)*scan_dir[i].y; 74 | // 计算哪个地图边与扫描方向相交 75 | std::vector pots; 76 | for(int j = 0; j < map.points.size(); j++) 77 | { 78 | cv::Point2d dir1 = map.points[j]-pos; 79 | cv::Point2d dir2 = map.points[j+1]-pos; 80 | if(cross_product(dir, dir1) * cross_product(dir, dir2) <= 0) 81 | { 82 | // 若相交，计算交点 83 | dir1 = map.points[j+1] - map.points[j]; 84 | Eigen::Matrix A; 85 | Eigen::Matrix b; 86 | A << dir.y, -dir.x, dir1.y, -dir1.x; 87 | b << pos.x*dir.y-pos.y*dir.x, map.points[j].x*dir1.y-map.points[j].y*dir1.x; 88 | Eigen::Matrix x = A.colPivHouseholderQr().solve(b); 89 | pots.push_back(cv::Point2d(x(0), x(1))); 90 | } 91 | } 92 | double min_dis = 100; 93 | for(int k = 0; k < pots.size(); k++) 94 | { 95 | cv::Point2d dir1 = pots[k] - pos; 96 | if(product(dir, dir1) < 0) continue; 97 | double dis = std::sqrt((pos.x-pots[k].x)*(pos.x-pots[k].x)+(pos.y-pots[k].y)*(pos.y-pots[k].y)); 98 | if(dis < min_dis) 99 | { 100 | min_dis = dis; 101 | scan_point[i] = pots[k]; 102 | data[i] = dis; 103 | } 104 | } 105 | } 106 | } 107 | std::vector data; 108 | std::vector angle; 109 | std::vector scan_dir; 110 | std::vector scan_point; 111 | }; 112 | 113 | class Imu 114 | { 115 | public: 116 | Imu(){ 117 | acc[0] = 0; 118 | acc[1] = 0; 119 | gyro = 0; 120 | } 121 | ~Imu(){} 122 | void add_noise() 123 | { 124 | std::normal_distribution noise(0.0, 0.01); 125 | acc[0] += noise(gen); 126 | acc[1] += noise(gen); 127 | gyro += noise(gen); 128 | } 129 | double acc[2]; 130 | double gyro; 131 | std::default_random_engine gen; 132 | }; 133 | 134 | class State 135 | { 136 | public: 137 | State(){ 138 | pos[0] = 0.5; 139 | pos[1] = 0.5; 140 | vel[0] = 0; 141 | vel[1] = 0; 142 | theta = 0; 143 | acc_bias[0] = 0; 144 | acc_bias[1] = 0; 145 | gyro_bias = 0; 146 | } 147 | ~State(){} 148 | void update(Eigen::Matrix delta) 149 | { 150 | pos[0] += delta(0); 151 | pos[1] += delta(1); 152 | vel[0] += delta(2); 153 | vel[1] += delta(3); 154 | theta += delta(4); 155 | acc_bias[0] += delta(5); 156 | acc_bias[1] += delta(6); 157 | gyro_bias += delta(7); 158 | } 159 | double pos[2]; 160 | double vel[2]; 161 | double theta; 162 | double acc_bias[2]; 163 | double gyro_bias; 164 | }; 165 | 166 | class Car 167 | { 168 | public: 169 | Car(){ 170 | current_time = cv::getTickCount(); 171 | P = Eigen::Matrix::Identity() * 0.001; 172 | Q = Eigen::Matrix::Identity() * 0.5; 173 | R = Eigen::Matrix::Identity(); 174 | Fx = Eigen::Matrix::Identity(); 175 | Fn = Eigen::Matrix::Zero(); 176 | H = Eigen::Matrix::Zero(); 177 | memcpy(new_pos, true_state.pos, 2*sizeof(double)); 178 | lidar_points.resize(8); 179 | } 180 | ~Car(){} 181 | void update_pos(double x, double y){ 182 | new_pos[0] = x/100.0; 183 | new_pos[1] = y/100.0; 184 | } 185 | void update_imu() 186 | { 187 | last_time = current_time; 188 | current_time = cv::getTickCount(); 189 | delta_time = (current_time-last_time)/cv::getTickFrequency(); 190 | new_vel[0] = (new_pos[0]-true_state.pos[0])/delta_time; 191 | new_vel[1] = (new_pos[1]-true_state.pos[1])/delta_time; 192 | double acc[2]; 193 | acc[0] = (new_vel[0]-true_state.vel[0])/delta_time; 194 | acc[1] = (new_vel[1]-true_state.vel[1])/delta_time; 195 | imu.acc[0] = std::cos(true_state.theta)*acc[0] + std::sin(true_state.theta)*acc[1]; 196 | imu.acc[1] = -std::sin(true_state.theta)*acc[0] + std::cos(true_state.theta)*acc[1]; 197 | imu.gyro = (new_theta - true_state.theta)/delta_time; 198 | memcpy(true_state.vel, new_vel, 2*sizeof(double)); 199 | memcpy(true_state.pos, new_pos, 2*sizeof(double)); 200 | true_state.theta = new_theta; 201 | imu.add_noise(); 202 | } 203 | void update_nominal() 204 | { 205 | nominal_state.pos[0] += nominal_state.vel[0]*delta_time; 206 | nominal_state.pos[1] += nominal_state.vel[1]*delta_time; 207 | double accx = imu.acc[0] - nominal_state.acc_bias[0]; 208 | double accy = imu.acc[1] - nominal_state.acc_bias[1]; 209 | nominal_state.vel[0] += (std::cos(nominal_state.theta)*accx - std::sin(nominal_state.theta)*accy)*delta_time; 210 | nominal_state.vel[1] += (std::sin(nominal_state.theta)*accx + std::cos(nominal_state.theta)*accy)*delta_time; 211 | nominal_state.theta += (imu.gyro-nominal_state.gyro_bias)*delta_time; 212 | // 更新先验协方差 213 | Eigen::Matrix a; 214 | Eigen::Matrix R; 215 | a << -accy, accx; 216 | R << std::cos(nominal_state.theta), -std::sin(nominal_state.theta), std::sin(nominal_state.theta), std::cos(nominal_state.theta); 217 | Fx.block<2,2>(0,2) = Eigen::Matrix::Identity() * delta_time; 218 | Fx.block<2,1>(2,4) = R*a*delta_time; 219 | Fx.block<2,2>(2,5) = -R*delta_time; 220 | Fx(4,7) = -delta_time; 221 | Fn(4,2) = -1; 222 | Fn.block<3,3>(5,3) = Eigen::Matrix::Identity(); 223 | Fn.block<2,2>(2,0) = -R; 224 | P = Fx*P*Fx.transpose() + Fn*Q*Fn.transpose(); 225 | } 226 | void update_estimation(Map& map) 227 | { 228 | // 构造H矩阵 229 | Eigen::Matrix R; 230 | Eigen::Matrix t; 231 | R << std::cos(nominal_state.theta), -std::sin(nominal_state.theta), std::sin(nominal_state.theta), std::cos(nominal_state.theta); 232 | t << nominal_state.pos[0], nominal_state.pos[1]; 233 | for(int i = 0; i < lidar.data.size(); i++) 234 | { 235 | // 将点投影到世界坐标系 236 | Eigen::Matrix p; 237 | p << lidar.data[i]*std::cos(lidar.angle[i]), lidar.data[i]*std::sin(lidar.angle[i]); 238 | p = R*p + t; 239 | // 寻找地图中最近的线段 240 | cv::Point2d dir; 241 | double dis = map.find_nearest(cv::Point2d(p(0), p(1)), dir); 242 | z(i) = dis; 243 | Eigen::Matrix n; 244 | n << dir.x, dir.y; 245 | p << -lidar.data[i]*std::sin(lidar.angle[i]), lidar.data[i]*std::cos(lidar.angle[i]); 246 | H.block<1,2>(i,0) = n; 247 | H(i,4) = n*R*p; 248 | } 249 | // 计算卡尔曼增益 250 | Eigen::Matrix I = Eigen::Matrix::Identity(); 251 | K = P*H.transpose()*(H*P*H.transpose()+I).inverse(); 252 | // 更新名义状态 253 | Eigen::Matrix deltaX = K*z; 254 | nominal_state.update(deltaX); 255 | // 更新后验协方差 256 | P = (I-K*H)*P*(I-K*H).transpose() + K*K.transpose(); 257 | // 可视化点云 258 | R << std::cos(nominal_state.theta), -std::sin(nominal_state.theta), std::sin(nominal_state.theta), std::cos(nominal_state.theta); 259 | t << nominal_state.pos[0], nominal_state.pos[1]; 260 | for(int i = 0; i < lidar.data.size(); i++) 261 | { 262 | // 将点投影到世界坐标系 263 | Eigen::Matrix p; 264 | p << lidar.data[i]*std::cos(lidar.angle[i]), lidar.data[i]*std::sin(lidar.angle[i]); 265 | p = R*p + t; 266 | lidar_points[i] = cv::Point2d(p(0), p(1)); 267 | } 268 | } 269 | void update_lidar(const Map& map) 270 | { 271 | lidar.measure(true_state.pos, true_state.theta, map); 272 | } 273 | Lidar lidar; 274 | Imu imu; 275 | State true_state; 276 | State nominal_state; 277 | State error_state; 278 | double new_pos[2]; 279 | double new_vel[2]; 280 | double new_theta; 281 | double current_time; 282 | double last_time; 283 | double delta_time; 284 | Eigen::Matrix P; 285 | Eigen::Matrix Q; 286 | Eigen::Matrix R; 287 | Eigen::MatrixXd Fx; 288 | Eigen::MatrixXd Fn; 289 | Eigen::Matrix H; 290 | Eigen::Matrix z; 291 | Eigen::Matrix K; 292 | std::vector lidar_points; 293 | }; 294 | 295 | #endif --------------------------------------------------------------------------------