SLAM과 cholesky 분해
나이 40 넘어서 수학에 대해서 다시 한번 생각하게 될 기회가 있게 될 줄은… 암튼 최근에 SLAM(Simultaneous Localization and Mapping)관련된 간단한 업무를 좀 하게 되다 보니 어쩔 수 없이 수학적인 내용을 좀 알아야 겠더라. 먼저 인수분해.. 중학교 때 배웠었나.. 이걸 영어로 Factorization. 위키백과를 보면 “어떤 원소를 다른 원소의 곱으로 표시하는 것”이라고 나와 있다.
그렇다. A를 인수분해하면 √A × √A 이렇게 쓸 수 있듯이.. SLAM을 하다보니 Cholesky decomposition(촐레스키 분해?)란게 나오던데 이게 뭘 하는건가 했다.. Decomposition은 Factorization과 같은 의미로 보고, 촐레스키 분해는 인수분해를 하는데 행렬(Matrix)를 인수분해 한다고 보면 쉬울 것 같다.
즉, A라는 matrix가 있을 때 A = L *L.transpose() 가 되는 matrix L을 구하는 것이다.
여기서 L은 Matrix 대각선을 중심으로 아래쪽만 값이 있고 위쪽 원소는 모두 “0”인 모양인 된다. L.transpose()는 L을 열과 행을 바꾼 전치 행렬이고.
C++에서 이걸 실습 해보면 좀 더 이해가 쉽다. Eigen 라이브러리를 활용하면 쉽게 해볼 수 있다.
#include <float.h>
#include <sys/types.h>
#include <vector>
#include <utility>
#include <cmath>
#include <iostream>
#include <random>#include <eigen3/Eigen/Dense> // /usr/include/ 아래 있는지 확인
#include <eigen3/Eigen/Core> // 경로는 시스템에 따라 다를 수 있음using namespace std;
using namespace Eigen;int main(int argc, char* argv[])
{
MatrixXd A(3,3);
A << 6개 숫자 입력, 예, 4, -1, ... ;
cout << "Matrix A is " << endl << A << endl; MatrixXd L = A.llt().matrixL(); //cholesky decomposition
cout << L * L.transpose() << endl; // A가 나옴.
return 0;
}
그럼 이걸 뭐 할 때 쓰는 건가? 촐레스키 분해는 몬테카를로 시뮬레이션에서 (위치 추정 할 때 이 방식을 많이 쓰는 것 같음. 파티클 필터 쓸 때도) 임의의 숫자(random number)를 추출 할 때, 추출한 random number가 correlation이 0이 아닌 원하는 correlation을 갖도록 수정 할 때 쓴다.
random number를 원소로 하는 matrix를 만들면 random이므로 원소들 간의 correlation은 0 인데, 여기에 L을 곱해 주면 correlation이 있는 원소들로 바꿀 수 있다.
즉, cholesky factorization(decompostion)은 상관관계를 가지는 2개 이상의 난수를 만들고자 할 때 사용하는 것인데, 내 경우를 대입 해 보니 현재 나는 차량의 뒤쪽 두 바퀴의 wheel speed를 값(VL, VR)을 받아서 위치 추정을 하고 있는데, 각 wheel speed 값에 가우시안 노이즈를 더하는 구조로 되어 있었다.
Q라는 2-by-2 matrix를 만들어서 Q(0, 0)과 Q(1,1)원소에 가우시안 노이즈를 넣고, (나머지 원소는 0) 이렇게 얻어진 Q matrix의 cholesky decomposition matrix을 구했다.
MatrixXf _S = Q.llt().matrixL();
// Q는 2-by-2 matrix
// _S는 대각선 아래만 값이 있는 2-by-2 행렬이 됨.여기에 MatrixXf _X(2,1); 행렬을 하나 만들어서 각 원소[_X(0,0), _X(1,0)]에 random값을 넣게 된다 .
Vector2f VLR = _S*_X + Vector2f(VL, VR);
VL = VLR(0); VR = VLR(1);// 여기에서 VL, VR값은 차량으로 부터 입력받은 Rear Left, Rear Right
// wheel speed 값 (float)
// 갱신된 VL, VR값은 노이즈가 추가된 값이 된다.
VL과 VR은 휠 속이니까 서로 상관 관계가 있을 것이고 _S*_X를 통해 상관성이 있는 random 값 가우시안 노이즈로 입력값에 더해 주는 효과를 주는 것이었다.
*참고: 위치추정(Localization)은 로봇이든 차량이든 이동체 제어에 중요한 기술인데, 센서 관측 정보 및 차량으로 부터 수신하는 odometry정보(wheel speed sensor, yaw 값 등)의 불확실성? 오차라고 하는게 맞겠다… 및 실시간성 때문에 어려운 문제이다. 말 그대로 추정(estimate)이기 때문에 예측하고 입력 받은 센서값 기반으로 보정하는 과정을 거치게 된다. 나의 경우 odometry정보를 기반으로 차량 위치를 예측하고, uwb 센서로 부터 받은 거리 정보(anchor와 차량에 장착된 tag사이의 거리)로 보정하는 구조로 되어 있다. anchor의 위치는 EKF(Extended Kalman filter)방법으로 추정하고, 차량의 위치는 particle filter로 추정하고 있다. 이러한 방식이 일반적으로 사용되는 RBPF(Rao-Blackwellized Particle Filter) 기반 SLAM이다.
