Microsoft PowerPoint - 6_칼만필터.ppt [호환 모드]

칼만필터살펴보기 이번에는칼만필터 (Kalman filter) 에대해서살펴보고자합니다. 인터넷에서칼만필터관련자료를찾아보면많은자료와응용이소개되어있습니다. 위키백과에칼만필터에대해서우리말로잘정리설명되어있습니다. http://ko.wikipedia.org/wiki/%ec%b9%bc%eb%a7%8c_%ed%95%84%ed%84%b0#.ec.b9.bc.eb.a7.8c_.ed.95.84.ed.84.b0.ec.9d.98_.ec.a0.81.ec.9a. A9_.EB.B6.84.EC.95.BC 영어로보시려면여기 (http://en.wikipedia.org/wiki/kalman_filter ) 를참조하시면됩니다. 그리고이곳 (http://www.cs.unc.edu/ edu/~welch/kalman/ ) 에도칼만필터관련자료가잘정리되어있네요. 아두이노관련소스를보시려면 http://arduino.cc/ 에서 kalman filter 를검색하시면몇개의소스들 ( 예 http://www.arduino.cc/cgibin/yabb2/yabb.pl?num=1253202298 ) 을볼수있습니다. 좀살펴봐서참조하시면됩니다. 저는이곳 ( http://www.arduino.cc/cgi-bin/yabb2/yabb.pl?num=1248889032/45 ) 에서 duckhead 의소스좀살펴보니마음에드네요. Duckhead 씨는눈척과모션플러스를아래사진과같이꾸며서실험했다하는데, 2 개의연결을아래오른쪽그림과같이연결해서했다네요. 이방법은 2개의트랜지스터를사용하여눈척과모션플러스를선택액세스하는데, 꾸며서동작시켜보니나름괜찮습니다. 작성 : 2010.11.06 KCO 1

칼만필터살펴보기 소스의머리말 : // Modified Kalman code using Roll, Pitch, and Yaw from a Wii MotionPlus and X, Y, and Z accelerometers from a Nunchuck. // Also uses "new" style Init to provide unencrypted data from the Nunchuck to avoid the XOR on each byte. // Requires the WM+ and Nunchuck to be connected to Arduino pins D3/D4 using the schematic from Knuckles904 and johnnyonthespot // See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html // Kalman Code By Tom Pycke. http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data // Original zipped source with very instructive comments: http://tom.pycke.be/file_download/4 // Some of this code came via contributions or influences by Adrian Carter, Knuckles904, evilbunny, Ed Simmons, & Jordi Munoz // There are two coordinate systems at play here: // The body-fixed coordinate system, which is fixed to the platform. That is, the x-axis is always pointing to the nose of the // aircraft (nosecone on a rocket), the y-axis points to the right, and the z-axis points down on an airplane, (parallel to the // earth away from you on a rocket). // The aircraft will move with respect to the navigation frame, which is a fixed coordinate system. // In that frame, the x-axis points north, the y-axis points east and the z-axis points down. This is called the "NED system", or // the "Local Tangent Plane" or the "Local Geodetic Frame". // Created by Duckhead v0.6 Yaw is done via integration of gyro data and smoothed via a Runge-Kutta 4, but there are still issues with it 하드웨어연결은이곳 (http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html ) 에서참조하시고, 칼만소스코드는여기 (http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data ) 에서참조했다하는데, 여러가지자료들이좀있고, 칼만필터관련자료로 http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf 와이론적인자료 http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf 를링크해줍니다. 또한 dspic30을사용한소스 KalmanTestBoardV1.zip 도링크해주는데, 아두이노소스와비슷합니다. 네이버에서칼만필터를검색해보면 http://kin.naver.com/qna/detail.nhn?d1id=11&dirid=1118&docid=57854287&qb=7lm866em7zwe7ysw&enc=utf8&section=kin&rank=1&search_s ort=0&spq=0&pid=glrnmwoi5tvsss4/aigsss--271841&sid=tnt2favw1ewaac15dr4 나름대로설명이잘되어있습니다. 저는아직이론적인지식이부족하여소스설명및기초이론은고수들님께서해주시기를기대합니다. 2

칼만필터쉬운설명 칼만필터는 1960년 R.E.Kalman (Kalman, Rudolph, Emil) 의논문 "A New Approach to Linear Filtering and Prediction Problems" 에그시초를두고있다. 여기서는처음칼만필터의내용을접하는초보자를위해한글로몇가지중요한내용을정리해본다. 칼만필터는흔히 " an optimal recursive data processing algorithm" 이라고불린다. 이에대한직접적인설명을하기전에간단한예로써그의미를전달해보자. 어느고등학교선생님이자신의학급학생들의수학점수평균을알고싶다고가정하자. 물론모든학생의점수를더한후이를학생수로나누는방법이가장쉽고간편할것이다. 그러나, 한가지재미를더하기위해상황을설정하자면아직선생님은학생들의점수를전혀알고있지못한상황이고, 학생들은한번에한명씩만선생님께점수를말해준다고가정하자. 이때선생님이평균을짐작해보기위한가장좋은방법은학생한명한명이점수를말할때마다이들의평균으로전체의평균을짐작해보는것이다. 즉다음과같은관계식을생각해볼수있다. X'(1) = X1 X'(2) = (X1+X2)/2 = (X'(1)*1+X2)/2 X'(3) = (X1+X2+X3)/3 = (X'(2)*2+X3)/3 X'(4) = (X1+X2+X3+X4)/4 = (X'(3)*3+X4)/4 : X'(n) = (X1+X2+...+Xn)/n = (X'(n-1)*(n-1)+Xn)/n 위식의일반식은다음과같이변형할수도있다. X'(n) = X'(n-1)*((n-1)/n) + (1/n)*Xn ; 손으로좀정리해보면나옵니다. 그럼위식이갖고있는특징은무엇일까? 위와같은식을사용할때의간편한점은지나간개개의값들을모두기억해둘필요가없다는것이다. 즉 n번째평균을구할때선생님은 X'(n-1) 과지금 n번째의값 Xn 두개의값만있으면아무문제없이 n번째의평균을구할수있게된다. 이와같은기법을 " 반복적자료처리 (recursive data processing)" 이라고하며, 최초 Kalman Filter 이론의개발이유이기도하다. 처리할자료의양이그리많지않을때는모든자료를기억해두었다가한번에계산하는것이간편할수도있겠으나, 그자료의양이방대하다면, 저장장소및처리시간을고려하지않을수없게된다. 칼만필터가개발된 60년대는컴퓨터의발달이초보적인상태였기에자료의효율적저장및처리가절실할시절임을감안한다면어쩌면당연한고안이라고생각할수있겠다. 그럼이제 " 최적 (optimal)" 이라는단어에초점을둬보자. 위에서제시한예는개개의데이터 ( 각학생의수학점수 ) 에동일한가중치, 즉새로받아들이는모든값에는 1/n의가중치가두어졌다. 그렇다면가중치가다른경우에는어떠할까? 같은점수를갖고있는학생 3명이함께와서 " 우리점수는ㅇㅇ점입니다." 라고한다면이점수에대한가중치는 3/n을주어야할것이다. 이와같이각데이터가갖고있는평균에대한중요도를 " 경중률 " 이라고한다. 경중률에대한개념은측량에있어서여러방법으로길이를재고이들의평균을구해봄으로써더욱쉽게이해할수있다. 3

칼만필터쉬운설명 이번에는위의수학선생님이학생두명에게각기다른방법으로교실의길이를재보라고했다고하자. 한학생은자신의보폭을이용하고, 다른학생은줄자를이용하여교실의길이를측정했을때, 첫번째학생은 10m, 두번째학생은 12m 라는값을얻었다. 만약둘모두같은방법으로길이를측정하였다면, 선생님은주저없이두값을평균하여 11m 를교실의길이라고말할수있겠다. 그러나, 누가보더라도보폭으로잰길이보다는줄자로잰길이가정확하다고느낄것이다. 그렇다면이들두값을어떻게평균해야할까? 이때도입되는개념이경중율이다. 측량학에있어서관측값의경중률은각값이갖고있는표준편차를기준으로정한다. 즉각관측치에대한경중률은표준편차제곱에반비례한다. 표준편차가크면클수록그값의경중률은떨어지며평균에대한기여도가적어지게된다. 줄자로잰거리가 10m 이지만, 수많은경험또는수많은반복측정을통해얻어진표준편차가 +-0.1m 라고가정한다면, 그리고보측은 12m +-1.0m라고가정한다면, 이들의평균은다음과같이구해진다. 평균 = (10*(1.0)^2 + 12*(0.1)^2 ) / (0.1^2+1.0^2) = 10.02m 위의식을일반화하면, X = (X1*(sd2)^2 + X2*(sd1)^2) / ((sd1)^2+(sd2)^2) = (X1*(sd1)^2 - X1*(sd1)^2 + X1*(sd2)^2 + X2*(sd1)^2) / ((sd1)^2+(sd2)^2) = X1 + (X2-X1)* [(sd1)^2 / ((sd1)^2+(sd2)^2)] (sd ) )] 여기서, X : 상태변수 (state variables) sd : 표준편차 (standard deviation) 위의마지막식이칼만필터에있어서시스템출력값 ( 위에서는 X1) 과새로운입력값 (X2) 을이용하여새로운최적값 (optimized state variables) 을계산하는 " 관측갱신알고리즘 (measurement update algorithm)" 의스칼라형태이다. ( 일반적으로칼만필터의기본식은행렬또는벡터형태로나타나있다.) 더불어새로운최적값, X, 의표준편차는다음과같이계산된다. (sd)^2 = [(sd1)^2+(sd2)^2] / [(sd1)^2*(sd2)^2] 이제위의두식, 최적값 X와표준편차 sd를구하는, 을이용하면앞에서말한두관측값이외에다른관측값을추가적으로얻어서다시새로운최적값을구할때도같은방법을계속사용할수있다. 이때는앞에서얻어진 X 는 X1 이되고, sd 는 sd1 이되며, 새로운관측값과표준편차 ( 경중률 ) 은각각 X2 와 sd2 가된다. 이와같은반복적인 (recursive) 연산 (data processing) 을통해최적 (optimal) 값을추적하는것이칼만필터의기본개념이라할수있겠다. 4

칼만필터쉬운설명 먼저시스템방정식과관측방정식을살펴보자. 칼만필터를도입하기위해서는기본적으로위와같은두선형방정식이필요하다. 비선형방정식에대한 " 확장형칼만필터 (Extended Kalman Filter)" 는비선형방정식을테일러급수전개등을이용하여선형화한후적용한형태일뿐이다. ( 다만비선형의선형화에따른변환계수 -그림에서 A 또는 H와같은- 의형태가달라질뿐이다. 이에대해서는 " 확장형칼만필터" 에서다시자세히설명하겠다.) 시스템방정식에서 x는우리의관심, 즉최적화를하고자하는상태변수를의미하고계수 A 는한단계에서의상태변수와다음단계의상태변수를연결하는변환계수를표현한다. B 와 u 는한덩어리로인식할수있으며이들은시스템에무관한추가입력값이다. 마지막으로 w 는 k 단계에서상태변수 x 의참값과의차이값또는 " 시스템오차 (system error or system noise)" 이다. ( 우리가알고있는 x 는참값에근접한계산값일뿐이다.)w 는개별적으로값을구하거나지정할수없으며, 단지오랜관측및시스템의제작시부터알고있는참값에대한표준편차로써 -칼만필터안에서는분산의형태로써- "Q" 라는변수로적용된다. 관측방정식에서 z 는관측값이고이는상태변수 x 와변환계수 H 에의해표현되며 v 는관측값 z 와관측참값과의오차 (measurement error or measurement noise) 이다. v 는 w 와마찬가지로개개의값을알수는없고관측참값에대한분산인 "R" 라는변수로써칼만필터안에서사용된다. 여기에서칼만필터의기본가정을소개하자면, 이는 [ 오차 w 및관측방정식에서의 v 는각각의참값에대해정규분포하며그평균은 0 이고분산은각각 Q 와 R 이다.] 이다. 이가정은칼만필터의가장큰장점인반면또한가장큰단점으로작용하기도한다. 시스템연산값및관측값이참값에대해정규분포하는일반적인경우에는위와같이간단한칼만필터알고리즘을통해최적화할수있는강력한도구가되지만, 그렇지않은경우 - 오차가참값에대해정규분포하지않는경우또는그렇다하더라도그분산을알수없는경우- 에는그적용에있어문제와어려움이크다. 그러나오차의확률분포가정규분포가아닌경우라하더라도이를정규분포로간주함에있어그리큰무리가없는경우에는칼만필터는아직까지유용하고강력한도구로사용된다.(??? 수정필요!!!) 위에설명한시스템방정식과관측방정식의이해를위해간단한예를들어본다면, 등속운동을하고있으며때때로추가적인가속또는감속을하고있는자동차를생각해보자. 이는기본적으로등속운동을하고있으니 k-1 단계에서의속도와 k 에서의속도는동일하므로 A는 "1" 이다. 또한추가적인가감속에따른속도변화는그것이이루어진단계에서 Bu 에그값을적용할수있다. 그리고그자동차를외부에서스피드건을이용해관측한다면이는그속도를직접관측하는것이므로 H 역시 "1" 이다. 이문구는인터넷블로그곳곳에서인용되고있네요. 5

예측, Update 수식 6

// Modified Kalman code using Roll, Pitch, and Yaw from a Wii MotionPlus and X, Y, and Z accelerometers from a Nunchuck. // Also uses "new" style Init to provide unencrypted data from the Nunchuck to avoid the XOR on each byte. // Requires the WM+ and Nunchuck to be connected to Arduino pins D3/D4 using the schematic from Knuckles904 and johnnyonthespot // See http://randomhacksofboredom.blogspot.com/2009/07/motionplus-and-nunchuck-together-on.html // Kalman Code By Tom Pycke. http://tom.pycke.be/mav/71/kalman- filtering-of-imu-data imu // Original zipped source with very instructive comments: http://tom.pycke.be/file_download/4 // Some of this code came via contributions or influences by Ed Simmons, Jordi Munoz, and Adrian Carter // Created by Duckhead v0.2 some cleanup and other optimization work remains. also need to investigate correlation of nunchuk and WMP (which axis is which) // simplify test by KCO 2010.11.06 #include <math.h> #include <Wire.h> ///////////////////////////////////////// struct GyroKalman{ These variables represent our state matrix x float x_angle, x_bias; Our error covariance matrix float P_00, P_01, P_10, P_11; * Q is a 2x2 matrix of the covariance. Because we * assume the gyro and accelerometer noise to be independent * of each other, the covariances on the / diagonal are 0. * Covariance Q, the process noise, from the assumption * x = F x + B u + w * with w having a normal distribution with covariance Q. * (covariance = E[ (X - E[X])*(X - E[X])' ] * We assume is linear with dt 아두이노소스 float Q_angle, Q_gyro; * Covariance R, our observation noise (from the accelerometer) * Also assumed to be linair with dt float R_angle; ; struct GyroKalman accx; float accelanglex=0; //NunChuck X angle float accelangley=0; //NunChuck Y angle float accelanglez=0; //NunChuck Z angle byte buf[6]; // array to store arduino output int cnt = 0; //Counter unsigned long lastread=0; * R represents the measurement covariance noise. In this case, * it is a 1x1 matrix that says that we expect 0.3 rad jitter * from the accelerometer. static const float R_angle = 0.5; //.3 default * Q is a 2x2 matrix that represents the process covariance noise. * In this case, it indicates how much we trust the acceleromter * relative to the gyros. static const float Q_angle = 0.01; //0.01 (Kalman) static const float Q_gyro = 0.04; //0.04 (Kalman) //These are the limits of the values I got out of the Nunchuk accelerometers (yours may vary). const int lowx = -215; const int highx = 221; const int lowy = -215; const int highy = 221; const int lowz = -215; const int highz = 255; 7

아두이노소스 * Nunchuk accelerometer value to degree conversion. Output is -90 to +90 (180 degrees of motion). float angleindegrees(int lo, int hi, int measured) { float x = (hi - lo)/180.0; return (float)measured/x; void nunchuck_init () //(nunchuck){ // Uses New style init - no longer encrypted so no need to XOR bytes later... might save some cycles Wire.beginTransmission (0x52); // transmit to device 0x52 Wire.send (0xF0); 0); // sends memory address Wire.send (0x55); // sends sent a zero. Wire.endTransmission (); // stop transmitting delay(100); Wire.beginTransmission (0x52);// transmit to device 0x52 Wire.send (0xFB); // sends memory address Wire.send (0x00); // sends sent a zero. Wire.endTransmission (); // stop transmitting void initgyrokalman(struct GyroKalman *kalman, const float Q_angle, const float Q_gyro, const float R_angle) { kalman->q_angle = Q_angle; kalman->q_gyro = Q_gyro; kalman->r_angle = R_angle; kalman->p_00 = 0; kalman->p_01 = 0; kalman->p >P_10 = 0; kalman->p_11 = 0; The kalman predict method. See http://en.wikipedia.org/wiki/kalman_filter#predict * kalman the kalman data structure * dotangle Derivitive Of The (D O T) Angle. This is the change in the angle from the gyro. This is the value from the * Wii MotionPlus, scaled to fast/slow. * dt the change in time, in seconds; in other words the amount of time it took to sweep dotangle * Note: Tom Pycke's ars.c code was the direct inspiration for this. However, his implementation of this method was inconsistent * with the matrix algebra that it came from. So I went with the matrix algebra and tweaked his implementation here. void odpedct(st predict(struct GyroKalman a *kalman, a float dotangle, float dt) { kalman->x_angle += dt * (dotangle - kalman->x_bias); kalman->p_00 += -1 * dt * (kalman->p_10 + kalman->p_01) + dt*dt * kalman->p_11 + kalman->q_angle; kalman->p_01 += -1 * dt * kalman->p_11; kalman->p_10 += -1 * dt * kalman->p_11; kalman->p_11 += kalman->q_gyro; The kalman update method. See http://en.wikipedia.org/wiki/kalman_filter#update * kalman the kalman data structure * angle_m the angle observed from the Wii Nunchuk accelerometer, in radians float update(struct GyroKalman *kalman, float angle_m) { const float y = angle_m - kalman->x_angle; const float S = kalman->p_00 + kalman->r_angle; const float K_0 = kalman->p_00 / S; const float K_1 = kalman->p >P_10 / S; kalman->x_angle += K_0 * y; kalman->x_bias += K_1 * y; kalman->p_00 -= K_0 * kalman->p_00; kalman->p_01 -= K_0 * kalman->p_01; kalman->p_10 -= K_1 * kalman->p_00; kalman->p_11 P11-= K1*k K_1 kalman->p_01; P01 return kalman->x_angle; 8

프로세싱소스 // 2010.11.06 KCO import processing.serial. serial *; Serial port; // Create object from Serial class float MAX_VAL = 65535.0; int val0,val1; // Data received from the serial port int[] values0; int[] values1; int gety(int val) { return (int)(val/max_val * (height-1)); void setup() { int tmp; size(800, 600); port = new Serial(this, "COM155", 115200); values0 = new int[width]; values1 = new int[width]; smooth(); int cr; int cnt=0,cnt_old=0; void draw(){ int tmp; while(port.available() >= 6){ tmp = port.read(); if(tmp=='k'){ val0 = (port.read() << 8) (port.read()); val1 = (port.read() << 8) (port.read()); cr = port.read(); cnt++; if(cnt!= cnt_old){ cnt_old = cnt; for (int i=0; i<width-1; i++){ values0[i] = values0[i+1]; values1[i] = values1[i+1]; 1] values0[width-1] = val0; values1[width-1] = val1; background(0); for (int x=1; x<width; x++) { stroke(255,0,0); // Red line(width-x, height-1-gety(values0[x-1]), width-1-x, height-1-gety(values0[x])); stroke(0,255,0); // Green line(width-x, height-1-gety(values1[x-1]), width-1-x, height-1-gety(values1[x])); 실행파형 : 적색 = 입력 X 값, 녹색 = 칼만필터처리값 9