ML Estimation of Process Noise Variance in Dynamic Systems

Transcription

1 ML Estimation of Process Noise Variance in Dynamic Systems Patrik Axelsson, Umut Orguner, Fredrik Gustafsson and Mikael Norrlöf Division of Automatic Control Department of Electrical Engineering Linköping University, Sweden

2 Outline 2(21) 1. Problem Formulation 2. Estimation of process noise covariance 3. Alternative Methods 4. Simulation Results

3 Outline 3(21) 1. Problem Formulation 2. Estimation of process noise covariance 3. Alternative Methods 4. Simulation Results

4 Background 4(21) The elasticity (in joints, arms,... ) for an industrial robot has increased over the last years. The motion control must be improved for these new robots. ABB IRB4600 (

5 Background 4(21) The elasticity (in joints, arms,... ) for an industrial robot has increased over the last years. The motion control must be improved for these new robots. One solution can be to estimate the tool position and use that estimate in the feedback loop. The tool position can be estimated with e.g. a EKF which uses knowledge about the process noise. ABB IRB4600 (

6 Background 4(21) The elasticity (in joints, arms,... ) for an industrial robot has increased over the last years. The motion control must be improved for these new robots. One solution can be to estimate the tool position and use that estimate in the feedback loop. The tool position can be estimated with e.g. a EKF which uses knowledge about the process noise. The accuracy is very important, < 1 mm. ABB IRB4600 (

7 Problem Formulation 5(21) Serial robot with two degrees of freedom. Nonlinear stiffness. Nonlinear friction. z b q a1 ρb q a2 z S P x S x b M(q) q + C(q, q) + G(q) + T(q) + D q + F( q) = u The model structure is common for mechanical systems derived by Newton s second law of motion or Lagrange s equation.

8 Problem Formulation 5(21) Serial robot with two degrees of freedom. Nonlinear stiffness. Nonlinear friction. z b q a1 ρb q a2 z S P x S x b M(q) q + C(q, q) + G(q) + T(q) + D q + F( q) = u The model structure is common for mechanical systems derived by Newton s second law of motion or Lagrange s equation. Accelerometer ( ) ρ s (q a, q a, q a ) = R b s(q a ) ρ b (q a, q a, q a ) + G b + δ s

9 Problem Formulation 6(21) cont d States x = ( q T a q T m qt a q T ) T m R 8 Discrete state space model (cont. disc. using Euler forward) x k+1 = F 1 (x k, u k ) + F 2 (x k )v k, v k N (0, Q) ( ) qm,k y k = + e k = h(x k, u k ) + e k, e k N (0, R) ρ s,k where F 2 (x k ) = ( ) 0. F 2 (x k ) All model parameters known except for the covariance matrix Q.

10 The Expectation Maximisation (EM) Algorithm 7(21) Maximum Likelihood (ML) method ˆθ ML = arg max log p θ (y 1:N ). θ Θ

11 The Expectation Maximisation (EM) Algorithm 7(21) Maximum Likelihood (ML) method ˆθ ML = arg max log p θ (y 1:N ). θ Θ Solution hard to find Expectation Maximisation (EM) algorithm.

12 The Expectation Maximisation (EM) Algorithm 7(21) Maximum Likelihood (ML) method ˆθ ML = arg max log p θ (y 1:N ). θ Θ Solution hard to find Expectation Maximisation (EM) algorithm. 1. Select an initial value θ 0 and set l = Expectation Step (E-step): Calculate Γ(θ; θ l ) = E θl [log p θ (y 1:N, x 1:N ) y 1:N ]. 3. Maximisation Step (M-step): Compute θ l+1 = arg max Γ(θ; θ l ). θ Θ 4. If converged, stop. If not, set l = l + 1 and go to step 2.

13 Outline 8(21) 1. Problem Formulation 2. Estimation of process noise covariance 3. Alternative Methods 4. Simulation Results

14 Estimation of process noise covariance 9(21) Estimate Q with the Expectation Maximisation (EM) algorithm.

15 Estimation of process noise covariance 9(21) Estimate Q with the Expectation Maximisation (EM) algorithm. Main ideas Calculate Γ(Q; Q l ) using linearisation. Use the smoothed states (EKS) as approximation when necessary. Possible since the smoothed densities are peaky when the SNR is high which is the case here.

16 Estimation of process noise covariance 10(21) Conditional densities x k+1 p(x k+1 x k ) = N y k p(y k x k ) = N (y k ; h k, R) ( ) x k+1 ; F 1,k, F 2,k QF T 2,k

17 Estimation of process noise covariance 10(21) Conditional densities x k+1 p(x k+1 x k ) = N y k p(y k x k ) = N (y k ; h k, R) The joint log likelihood function L Q (y 1:N, x 1:N ) = log p Q (y 1:N, x 1:N ) = L ( ) x k+1 ; F 1,k, F 2,k QF T 2,k N ( (x i F 1,i 1 ) T F 2,i 1 QF2,i 1) T (xi F 1,i 1 ) i=2 N i=2 log ( ( λ j F 2,i 1 QF2,i 1) ) T λ j =0

18 Estimation of process noise covariance 10(21) Conditional densities x k+1 p(x k+1 x k ) = N y k p(y k x k ) = N (y k ; h k, R) The joint log likelihood function L Q (y 1:N, x 1:N ) = log p Q (y 1:N, x 1:N ) = L ( ) x k+1 ; F 1,k, F 2,k QF T 2,k N ( (x i F 1,i 1 ) T F 2,i 1 QF2,i 1) T (xi F 1,i 1 ) i=2 N i=2 ( ) ) log ( F 2,i 1Q F T 2,i 1

19 Estimation of process noise covariance 11(21) cont d Expectation of the joint log likelihood function Γ(Q; Q l ) = E Ql [L Q (y 1:N, x 1:N ) y 1:N ] = L + 1 N ( ) ) ] 2 E Ql [log ( F 2,i 1Q F T 2,i 1 y 1:N i=2 1 N [ ( ) ] 2 tr E Ql F 2,i 1 QF T 2,i 1 (xi F 1,i 1 ) (x i F 1,i 1 ) T y 1:N i=2

20 Estimation of process noise covariance 12(21) cont d Calculation of the first expectation E Ql [log = ( ) ) ] ( F 2,i 1Q F T 2,i 1 y 1:N ( ) ) log ( F 2 (x i 1)Q F T 2 (x i 1) p Ql (x i 1 y 1:N ) dx i 1

21 Estimation of process noise covariance 12(21) cont d Calculation of the first expectation E Ql [log = ( ) ) ] ( F 2,i 1Q F T 2,i 1 y 1:N ( ) ) log ( F 2 (x i 1)Q F T 2 (x i 1) p Ql (x i 1 y 1:N ) dx i 1 Cannot be solved Approximation: Use the smoothed states. ( ) ) ] E Ql [log ( F 2,i 1Q F T 2,i 1 y 1:N ( ) ) log ( F 2 (ˆx s i 1 N )Q F T 2 (ˆxs i 1 N )

22 Estimation of process noise covariance 13(21) cont d Calculation of the second expectation [ ( ) ] E Ql F 2,i 1 QF T 2,i 1 (xi F 1,i 1 ) (x i F 1,i 1 ) T y 1:N ( ) = F 2 (x i 1 )QF T 2 (x i 1) (xi F 1,i 1 ) (x i F 1,i 1 ) T p Ql (x i, x i 1 y 1:N ) dx i dx i 1

23 Estimation of process noise covariance 13(21) cont d Calculation of the second expectation [ ( ) ] E Ql F 2,i 1 QF T 2,i 1 (xi F 1,i 1 ) (x i F 1,i 1 ) T y 1:N ( ) = F 2 (x i 1 )QF T 2 (x i 1) (xi F 1,i 1 ) (x i F 1,i 1 ) T p Ql (x i, x i 1 y 1:N ) dx i dx i 1 Use the smoothed states again [ ( ) ] E Ql F 2,i 1 QF T 2,i 1 (xi F 1,i 1 ) (x i F 1,i 1 ) T y 1:N ) (F 2 (ˆx s i 1 N )QFT 2 (ˆxs i 1 N ) (x i F 1,i 1 ) (x i F 1,i 1 ) T p Ql (x i, x i 1 y 1:N ) dx i dx i 1

24 Estimation of process noise covariance 14(21) cont d A first order Taylor approximation gives M = (x i F 1,i 1 ) (x i F 1,i 1 ) T p Ql (x i, x i 1 y 1:N ) dx i dx i 1 = ( J 1 I ) P ξ,s ( J1 I ) T i N ) ) T + (ˆx s i N F 1(ˆx s i 1 N (ˆx ) s i N F 1(ˆx s i 1 N ) J 1 = F 1(x) x P ξ,s i N x=ˆx s i 1 N is the smoothed state covariance for the augmented state vector ξ = ( x T i 1 x T ) T i

25 Estimation of process noise covariance 15(21) cont d Finally where Γ(Q; Q l ) = L + N 1 2 W = N i=2 log Q tr Q 1 W F 2(ˆx s i 1 N )M ( F 2(ˆx s i 1 N ) ) T

26 Estimation of process noise covariance 15(21) cont d Finally where Γ(Q; Q l ) = L + N 1 2 W = N i=2 log Q tr Q 1 W F 2(ˆx s i 1 N )M ( F 2(ˆx s i 1 N ) ) T Maximisation of Γ(Q; Q l ) w.r.t. Q gives Q l+1 = 1 N 1 N i=2 F 2(ˆx s i 1 N )M ( F 2(ˆx s i 1 N ) ) T

27 Outline 16(21) 1. Problem Formulation 2. Estimation of process noise covariance 3. Alternative Methods 4. Simulation Results

28 Alternative Methods 17(21) Alt. 1 Minimisation of the estimation error, given by e.g. EKF, w.r.t. Q, where Q is parametrised as a diagonal matrix. Alt. 2 Calculate v k from the state space model.

29 Alternative Methods 17(21) Alt. 1 Minimisation of the estimation error, given by e.g. EKF, w.r.t. Q, where Q is parametrised as a diagonal matrix. Alt. 2 Calculate v k from the state space model. 1. Select an initial value Q 0 and set l = Use the EKS with Q l. 3. Calculate the noise according to v k = F 2 ( ˆx s k N ) ( ˆx s k+1 N F 1 ( )) ˆx s k N, u k. 4. Let Q l+1 be the covariance matrix for v k according to Q l+1 = 1 N N v T k v k. k=1 5. If converged, stop, If not, set l = l + 1 and go to step 2.

30 Outline 18(21) 1. Problem Formulation 2. Estimation of process noise covariance 3. Alternative Methods 4. Simulation Results

31 Simulation Results 19(21) cont d Monte Carlo simulations on one simulated path with different initial values. No true values for Q Use Q in an EKF and calculate the estimation error (RMSE) for the path.

32 Simulation Results 19(21) cont d Monte Carlo simulations on one simulated path with different initial values. No true values for Q Use Q in an EKF and calculate the estimation error (RMSE) for the path. The EM algorithm converges to the same RMSE for all initial values. Same thing happens for Alt. 2. Alt. 1 ends up in different errors. The EM algorithm gives the lowest RMSE (2-norm of the RMSE). Max Min EM Alt Alt

33 Simulation Results cont d 20(21) EM Alg. 2 Alg Path error [mm] Time [s]

34 ML Estimation of Process Noise Variance in Dynamic Systems Patrik Axelsson, Umut Orguner, Fredrik Gustafsson and Mikael Norrlöf Division of Automatic Control Department of Electrical Engineering Linköping University, Sweden