Understanding and Application of Kalman Filter

Transcription

1 Dept. of Aerospace Engineering April 2, 2016

2 Contents I Introduction II Kalman Filter Algorithm III Simulation IV Future plans

3 Kalman Filter? Rudolf E.Kálmán(1930 ) 1960 년대루돌프칼만에의해 개발 Nosie 가포함된역학시스템 상태를재귀필터를이용해 참값과가까운값을추적신호처리, 로봇공학, 인공위성 등의여러분야에사용 종류 : Linear Kalman Filter Extended Kalman Filter(EKF) Uncented Kalman Filter(UKF)

4 Kalman Filter Recursive Algorithm EKF(Extended Kalman Filter) Ax k f (x k ) Hx k h(x k ) Kalman Filter Algorithm

5 Kalman Filter values 1)System model x: 상태변수 (nx1) 열벡터 A(= F, Φ): 상태전이행렬 (nxn) B: Control matrix(nx1) H: 측정값과상태변수의관계 (mxn행렬) Z: 측정값 (mx1) 행렬

6 Kalman Filter values 2)Linearization Jacobian Matrix( 비선형모델 선형모델 ) :n개의변수를가진함수 m개를모두편미분한행렬 F 1 (x 1,..., x n ),..., F m (x 1,..., x n ) J = KF EKF System Ax k f (x k ) Measurement Hx k h(x k ) A = f x ˆx k F 1 x 1 F 1 x n..... F m x 1 Fm x n H = h x ˆx k

7 Kalman Filter values 3)Noise 가우시안노이즈 : 평균이 0이고표준편차가 1인표준정규분포를따르는잡음

8 Kalman Filter values 4) 오차공분산 (P) : 필터의추정값과참값의차이를나타내는척도 P k = AP k 1A T + Q P k = (I K k H)P k P k = E{(x k ˆx k )(x k ˆx k ) T } 오차공분산은시간이지날수록줄어든다. 추정값은오차가충분히작아지면거의줄어들지않는다.

9 Kalman Filter values 5)Kalman gain 추정값을계산할때사용되는가중치

10 Example: Linear Filter Algorithm 등가속도운동 (uniformly accelerated motion) 1. 필터초기값설정 1) 시스템모델 (X k = AX k 1 + Bu) s = s 0 + vt at2 v = v 0 + at a = Const Ẋ = P V a X k = 1 dt X k 1 + dt 2 2 dt 0 X (3)

11 Example: Linear Filter Algorithm 등가속도운동 (uniformly accelerated motion) 1. 필터초기값설정 2) 측정모델 (z k = Hx k + v k ) z k = P V a = x(1) + x(2)dt + x(3)dt 2 /2 x(2) + x(3)dt x(3) + v k H = eye(3) v k = 측정잡음, (mx1) 열벡터

12 Example: Linear Filter Algorithm 등가속도운동 (uniformly accelerated motion) 1. 필터초기값설정 3)Noise Q = R = w k = Q randn(3, 1) v k = R randn(3, 1) P = 10 eye(3)

13 Simulation with matlab 1)Initial values 1 dt=0.01; %step size 2 t=0:dt:10; 3 x=[ ]'; %Initial values 4 xh=[0 0 0]'; 5 A=[1 dt 0; 0 1 0; 0 0 1]; %State Matrix 6 B=[dtˆ2/2; dt; 0]; %Control Matrix 7 P=10*eye(3); % 8 Q=diag([0.09ˆ2 0.02ˆ2 0.01ˆ2]); % 9 R=diag([3ˆ2 1.5ˆ2 0.5ˆ2]); % 10 H=eye(3); %

14 Simulation with matlab 2)Measurement and Kalman Filter 1 for i=1:length(t) 2 x=rk4(x,dt); %Runge kutta method 3 v=sqrt(r)*randn(3,1); %Measurement Noise 4 pm=x(1)+x(2)*dt+x(3)*dtˆ2/2+v(1); %Position+noise 5 vm=x(2)+x(3)*dt+v(2); %Velocity+noise 6 am=x(3)+v(3); %Acceleration+noise 7 z=[pm vm am]'; %Measurement values

15 Simulation with matlab 2)Measurement and Kalman Filter 1 w=sqrt(q)*randn(3,1); %System Noise 2 xh=a*xh+b*xh(3)+w; %Project the state ahead 3 P=A*P*A' + Q; %Project the error covariance ahead 4 K=P*H'*inv(H*P*H'+R); %Compute the Kalman Gain 5 xh=xh+k*(z-h*xh); %Update the estimate via z 6 P=(eye(3)-K*H)*P; %Update the error covariance 7 end

16 Simulation with matlab 3)Reasult 400 Measurement values 300 Position(m) time(sec) Velocity(m/s) Acceleration(m/s 2 )

17 Simulation with matlab 3)Reasult 400 Estimated value 300 Position(m) time(sec) Velocity(m/s) Acceleration(m/s 2 )

18 Simulation with matlab 3)Reasult 400 Estimated values and True values 300 KF True Position(m) time(sec) KF True Velocity(m/s) time(sec) 2.2 Acceleration(m/s 2 ) KF True 1.6 time(sec)

19 Simulation with matlab 3)Reasult 2 Errors Position error time(sec) 1 Velocity error time(sec) 1 Acceleration error Error 3σ -1 time(sec)

20 EKF Example (Van Der Pol s equation) Van Der Pol s equation: mẍ + 2c(x 2 1)ẋ + kx = 0 state variable: x = [x ẋ] T ẋ = [ẋ ẍ] T x 1 = x 2 x 2 = 2(c/m)(x1 2 1)x 2 (k/m)x 1 Thus, The linearized model is given by F = [ 0 1 4(c/m) ˆx 1 ˆx 2 (k/m) 2(c/m)(ˆx 2 1 1) ]

21 EKF Example (Van Der Pol s equation) 4 True values 3 x(1) x(2) Estimated values

22 EKF Example (Van Der Pol s equation) 0.3 3sigma

23 EKF Example (Van Der Pol s equation) 문제점 1 F = eye(2)+[0 1; 2-4*(c/m)*xh(1)*xh(2)-(k/m)... -2*(c/m)*(xh(1)ˆ2-1)]*dt; 3 xh = F*xh; 4 %xh=fx(xh,dt); 5 P = F*P*F'+ G*Q*G'; 6 K=P*H'*(H*P*H'+R)ˆ(-1); 7 xh=xh+k*(z-h*xh); 8 P=(eye(2)-K*H)*P; 이산시스템에대한개념부족 Discrete-Time, Continuous- Discrete 알고리즘구현방법의차이코드간결화문제

24 Future plan EKF 알고리즘및정확한시스템모델을이해, 동역학적인 문제적용 UKF 알고리즘구현

25 Thank you for your attention!