Lecture 9: Modeling and motion models

Transcription

1 Sensor Fusion, 2014 Lecture 9: 1 Lecture 9: Modeling and motion models Whiteboard: Principles and some examples. Slides: Sampling formulas. Noise models. Standard motion models. Position as integrated velocity, acceleration,... in nd. Orientation as integrated angular speed in 2D and 3D. Odometry (Extended) Kalman filter applications

2 Sensor Fusion, 2014 Lecture 9: 2 Lecture 8: Summary Simultaneous Localization And Mapping (SLAM) Joint estimation of trajectory x 1:k and map parameters θ in sensor model y k = h(x k ; θ) + e k. Algorithms: NLS SLAM: iterate between filtering and mapping EKF-SLAM: EKF (information form) on augmented state vector z k = (xk T, θt ) T. FastSLAM: MPF on augmented state vector zk = (xk T, θt ) T.

3 Sensor Fusion, 2014 Lecture 9: 3 Modeling First problem: physics give continuous time model, filters require (linear or nonlinear) discrete time model: Classification Nonlinear Linear Continuous time ẋ = a(x, u) + v ẋ = Ax + Bu + v y = c(x, u) + e y = Cx + Du + e Discrete time x k+1 = f (x, u) + v x k+1 = Fx + Gu + v y = h(x, u) + e y = Hx + Ju + e But first, sampling overview.

4 Sensor Fusion, 2014 Lecture 9: 4 Sampling formulas LTI state space model ẋ = Ax + Bu, y = Cx + Du, x k+1 = Fx + Gu y k = Hx + Ju. u is either input or process noise (then J denotes cross-correlated noise!). Zero order hold sampling assuming the input is piecewise constant: T x(t + T ) = }{{} e AT x(t) + e Aτ B u(t + T τ)dτ 0 F }{{} T = e AT x(t) + e Aτ dτbu(t). 0 First order hold sampling assuming the input is piecewise linear is another option. G

5 Sensor Fusion, 2014 Lecture 9: 5 Sampling formulas Bilinear transformation assumes band-limited input gives 2 z 1 x(t) sx(t) = Ax + Bu T z + 1 M = (I nx T /2A) 1, F = M(I nx + T /2A), G = T /2MB, H = CM, J = D + HG.

6 Sensor Fusion, 2014 Lecture 9: 6 Sampling of nonlinear models There are two options: Discretized linearization (general): 1. Linearization: A = a x(x, u), B = a u(x, u), C = c x(x, u), D = c u(x, u), 2. Sampling: F = e AT, G = T 0 eat dtb, H = C and J = D. Linearized discretization (best, when possible!): 1. Nonlinear sampling: t+t x(t + T ) = f (x(t), u(t)) = x(t) + a(x(τ), u(τ))dτ, t 2. Discretization:F = f x (x k, u k ) and G = f u(x k, u k ).

7 Sensor Fusion, 2014 Lecture 9: 7 Sampling of state noise a v t is continuous white noise with variance Q. T Q a = e f τ Qe (f ) T τ dτ 0 b v t = v k is a stochastic variable which is constant in each sample interval and has variance Q/T. Q b = 1 T T 0 e f τ dτq T 0 e (f ) T τ dτ c v t is a sequence of Dirac impulses active immediately after a sample is taken. Loosely speaking, we assume ẋ = f (x) + k v kδ kt t where v k is discrete white noise with variance TQ. Q c =Te f T Qe (f ) T T

8 Sensor Fusion, 2014 Lecture 9: 8 d v t is white noise such that its total influence during one sample interval is TQ. Q d =TQ e v t is a discrete white noise sequence with variance TQ. That is, we assume that the noise enters immediately after a sample time, so x(t + T ) = f (x(t) + v(t)). Q e =Tg (x)qg (x) T Recommendation: the simple solution in d works fine, but remember the scaling with T!

9 Sensor Fusion, 2014 Lecture 9: 9 Models Continuous time (physical) and discrete time counterparts ẋ(t) = a(x(t), u(t), w(t); θ), x(t + T ) = f (x(t), u(t), w(t); θ, T ). Kinematic models do not attempt to model forces. Black-box multi-purpose. Translation kinematics describes position, often based on F = ma Rotational kinematics describes orientation. Rigid body kinematics combines translational and rotational kinematics. Constrained kinematics. Coordinated turns (circular path motion). Application specific force models. Gray-box models, where θ has to be calibrated (estimated, identified) from data.

10 Sensor Fusion, 2014 Lecture 9: 10 Translational motion with n integrators Translational kinematics models in nd, where p(t) denotes the position X (1D), X, Y (2D) and X, Y, Z (3D), respectively. Another interpretation is p(t) = ψ(t) or p(t) = (φ(t), θ(t), ψ(t)) T. The signal w(t) is process noise for a pure kinematic model and a motion input signal in position, velocity and acceleration, respectively, for the case of using sensed motion as an input rather than as a measurement. State Continuous time ẋ = Discrete time x(t + T ) = x = p w x + Tw ( ) ( ) ( ) ( ) ( ) p 0n I x = n 0n In TI T 2 x + w n x + 2 In w v 0 n 0 n I n 0 n I n TI n x = p 0n In 0n v 0 n 0 n I n x + 0n 0 n w In TIn 2 T 2 In T 3 0 n I n TI n 6 In x + T 2 2 a 0 n 0 n 0 n I In w n 0 n 0 n I n TI n

11 Sensor Fusion, 2014 Lecture 9: 11 Different sampled models of a double integrator State x(t) = (p(t), v(t)) ( ) 0n I n Continuous time A = 0 n 0 ( n ) In TI n ZOH F = 0 n I ( n ) In TI n FOH F = 0 n I ( n ) In TI n BIL F = 0 n I n ( ) 0n B = I ( n T 2 ) G = 2 In ( TI n ) T 2 I n G = TI n ) G = ( T 2 4 In T 2 In C = (I n, 0 n) H = (I n, 0 n) D = 0 n J = 0 n H = (I n, 0 n) J = T 2 6 In H = ( I n, ) T 2 In J = T 2 2 In

12 Sensor Fusion, 2014 Lecture 9: 12 Navigation models Navigation models have access to inertial information. 2D orientation (course, or yaw rate) much easier than 3D orientation

13 Sensor Fusion, 2014 Lecture 9: 13 Rotational kinematics in 2D The course, or yaw, in 2D can be modeled as integrated white noise ψ(t) = w(t), or any higher order of integration. Compare to the tables for translational kinematics with p(t) = ψ and n = 1.

14 Sensor Fusion, 2014 Lecture 9: 14 Coordinated turns in 2D body coordinates Basic motion equations ψ = v x R = v xr 1, a y = v 2 x R = v 2 x R 1 = v x ψ, a x = v x v y v x R = v x v y v x R 1 = v x v y ψ. can be combined to a model suitable for the sensor configuration at hand. For instance, ( ) ψ x = R 1, u = v x, y = R 1 ( vx R ẋ = f (x, u) + w = 1 ) + w 0 is useful when speed is measured, and a vision system delivers a local estimate of (inverse) curve radius.

15 Sensor Fusion, 2014 Lecture 9: 15 Wheel speed sensor Ideal Toothed wheel Unideal Toothed Wheel α δ i Sensor Each tooth passing the sensor (electromagnetic or Hall) gives a pulse. The number n of clock cycles between a number m of teeth are registered within each sample interval. ω(t k ) = 2π N cog (t k t k 1 ) = 2π m N cog T c n. Problems: Angle and time quantization. Synchronization. Angle offsets δ in sensor teeth.

16 Sensor Fusion, 2014 Lecture 9: 16 Virtual sensors Longitudinal velocity, yaw rate and slip on left and right driven wheel (front wheel driven assumed) can be computed from wheel angular speeds if the radii are known: v x = v 3 + v 4 = ω 3r 3 + ω 4 r 4, 2 2 Ψ = ω 3r 3 ω 4 r 4, B s 1 = ω 1r 1 1, s 2 = ω 2r 2 1, v 1 v 2 ) 2 + (R 1 L) 2, ( v 1 = v x 1 + R 1 B 2 ( ) v 2 = v x 1 R 1 B 2 + (R 2 1 L) 2.

17 Sensor Fusion, 2014 Lecture 9: 17 Geometry The formulas are based on geometry, the relation ψ = v x R 1 and notation below. δ 2 α v 2 2 ω 1 ω 2 L ψ R1 R2 B ω 3 R 3 ω 4 R 4

18 Sensor Fusion, 2014 Lecture 9: 18 Odometry Odometry is based on the virtual sensors and the model vx m = ω 3r 3 + ω 4 r 4 2 ω 3 r 3 ψ m =vx m 2 ω 4 r4 1 ω B 3 r 3 ω 4 r t ψ t = ψ 0 + ψ tdt, 0 t X t = X 0 + vt x cos(ψt)dt, 0 t Y t = Y 0 + vt x sin(ψt)dt. 0 to dead-reckon the wheel speeds to a relative position in the global frame. The position (X t (r 3, r 4 ), Y t (r 3, r 4 )) depends on the values of wheel radii r 3 and r 4. Further sources of error come from wheel slip in longitudinal and lateral direction. More sensors needed for navigation.

19 Sensor Fusion, 2014 Lecture 9: 19 Rotational kinematics in 3D Much more complicated in 3D than 2D! Could be a course in itself. Coordinate notation for rotations of a body in local coordinate system (x, y, z) relative to an earth fixed coordinate system: Motion components Rotation Euler angle Angular speed Longitudinal forward motion x Roll φ ω x Lateral motion y Pitch θ ω y Vertical motion z Yaw ψ ω z

20 Sensor Fusion, 2014 Lecture 9: 20 Euler orientation in 3D An earth fixed vector g (for instance the gravitational force) is in the body system oriented as Qg, where Q = Qφ x Qy θ Qz ψ = cos θ 0 sin θ cos ψ sin ψ 0 0 cos φ sin φ sin ψ cos ψ 0 0 sin φ cos φ sin θ 0 cos θ cos θ cos ψ cos θ sin ψ sin θ = sin φ sin θ cos ψ cos φ sin ψ sin φ sin θ sin ψ + cos φ cos ψ sin φ cos θ. cos φ sin θ cos ψ + sin φ sin ψ cos φ sin θ sin ψ sin φ cos ψ cos φ cos θ Note: this result depends on the order of rotations Q x φ Qy θ Qz ψ. Here, the xyz rule is used, but there are other options.

21 Sensor Fusion, 2014 Lecture 9: 21 Euler rotation in 3D When the body rotate with ω, the Euler angles change according to ω x φ 0 0 ω y = 0 + Qφ x θ + Qφ x Qy θ 0 ω z 0 0 ψ The dynamic equation for Euler angles can be derived from this as φ 1 sin(φ) tan(θ) cos(φ) tan(θ) ω x ψ = 0 cos(φ) sin(φ) ω y θ 0 sin(φ) sec(θ) cos(φ) sec(θ) ω z Singularities when θ = ±π/2, can cause numeric divergence!

22 Sensor Fusion, 2014 Lecture 9: 22 Unit quaternions Vector representation: q = (q 0, q 1, q 2, q 3 ) T. Norm constraint of unit quaternion: q T q = 1. Then, the quaternion is defined from the Euler angles as q 0 = cos(φ/2) cos(θ/2) cos(ψ/2) + sin(φ/2) sin(θ/2) sin(ψ/2), q 1 = sin(φ/2) cos(θ/2) cos(ψ/2) cos(φ/2) sin(θ/2) sin(ψ/2), q 2 = cos(φ/2) sin(θ/2) cos(ψ/2) + sin(φ/2) cos(θ/2) sin(ψ/2), q 3 = cos(φ/2) cos(θ/2) sin(ψ/2) sin(φ/2) sin(θ/2) cos(ψ/2). Pros: no singularity, no 2π ambiguity. Cons: more complex and non-intuitive algebra, the norm constraint is a separate challenge. Norm constraint can be handled by projection or as a virtual measurement with small noise.

23 Sensor Fusion, 2014 Lecture 9: 23 Quaternion orientation in 3D The orientation of the vector g in body system is Qg, where q0 2 + q2 1 q2 2 q2 3 2q 1 q 2 2q 0 q 3 2q 0 q 2 + 2q 1 q 3 Q = 2q 0 q 3 + 2q 1 q 2 q0 2 q2 1 + q2 2 q2 3 2q 0 q 1 + 2q 2 q 3 2q 0 q 2 + 2q 1 q 3 2q 2 q 3 + 2q 0 q 1 q0 2 q2 1 q2 2 + q2 3 2q q q 1q 2 2q 0 q 3 2q 1 q 3 + 2q 0 q 2 = 2q 1 q 2 + 2q 0 q 3 2q q q 2q 3 2q 0 q 1. 2q 1 q 3 2q 0 q 2 2q 2 q 3 + 2q 0 q 1 2q q2 3 1

24 Sensor Fusion, 2014 Lecture 9: 24 Quaternion rotation in 3D Rotation with ω gives a dynamic equation for q which can be written in two equivalent forms: q = 1 2 S(ω)q = 1 2 S(q)ω, 0 ω x ω y ω z S(ω) = ω x 0 ω z ω y ω y ω z 0 ω x, ω z ω y ω x 0 q 1 q 2 q 3 S(q) = q 0 q 3 q 2 q 3 q 0 q 1. q 2 q 1 q 0

25 Sensor Fusion, 2014 Lecture 9: 25 Sampled form of quaternion dynamics The ZOH sampling formula q(t + T ) = e 1 2 S(ω(t))T q(t). has actually a closed form solution ( ω(t)t q(t + T ) = cos 2 ) I 4 + T sin 2 (I 4 + T2 S(ω(t)) ) q(t). ( ) ω(t) T 2 ω(t) T 2 S(ω(t)) q(t) The approximation coincides with Euler forward sampling approximation, and has to be used in more complex models where e.g. ω is part of the state vector.

26 Sensor Fusion, 2014 Lecture 9: 26 Double integrated quaternion ( ) q(t) = ω(t) ( 1 2 S(ω(t))q(t) ). w(t) There is no known closed form discretized model. However, the approximate form can be discretized using the chain rule to ( ) (( q(t + T ) T I4 2 S(ω(t))) T S(q(t)) ) ( ) 2 q(t) ω(t + T ) I 3 ω(t) }{{} F (t) ( T S(ω(t))I ) 4 v(t). TI 3

27 Sensor Fusion, 2014 Lecture 9: 27 Rigid body kinematics A multi-purpose model for all kind of navigation problems in 3D (22 states). ṗ v ȧ q ω ḃ acc ḃ gyro 0 I I = S(ω) p v a q ω b acc b gyro v a v ω v acc v gyro Bias states for the accelerometer and gyro have been added as well.

28 Sensor Fusion, 2014 Lecture 9: 28 Sensor model for kinematic model Inertial sensors (gyro, accelerometer, magnetometer) are used as sensors. y acc t y mag t y gyro t = R(q t )(a t g) + b acc t = R(q t )m + b mag t = ω t + b gyro t + et acc, et acc + e mag t, e mag t + e gyro t, e gyro t N ( 0, Rt acc ), N ( 0, Rt mag ), ), N ( 0, R gyro t Bias observable, but special calibration routines are recommended: Stand-still detection. When yt acc g and/or yt gyro 0, the gyro and acc bias is readily read off. Can decrease drift in dead-reckoning from cubic to linear. Ellipse fitting. When waving the sensor over short time intervals, the gyro can be integrated to give accurate orientation, and acc and magnetometer measurements can be transformed to a sphere from an ellipse.

29 Sensor Fusion, 2014 Lecture 9: 29 Tracking models Navigation models have access to inertial information, tracking models have not. Orientation mainly the direction of the velocity vector. Course (yaw rate) critical parameter. Not so big difference of 2D and 3D cases.

30 Sensor Fusion, 2014 Lecture 9: 30 Coordinated turns in 2D world coordinates Cartesian velocity Ẋ = v X Ẏ = v Y v X = ωv Y v Y = ωv X ω = ω v Y 0 0 ω 0 v X X t+t = X + v X ω sin(ωt ) v Y (1 cos(ωt )) ω Y t+t = Y + v X ω (1 cos(ωt )) + v Y ω sin(ωt ) vt+t X = v X cos(ωt ) v Y sin(ωt ) vt+t Y = v X sin(ωt ) + v Y cos(ωt ) ω t+t = ω Polar velocity Ẋ = v cos(h) Ẏ = v sin(h) v = 0 ḣ = ω ω = cos(h) v sin(h) sin(h) v cos(h) X t+t = X + 2v ω sin( ωt 2 ) cos(h + ωt 2 ) Y t+t = Y 2v ω sin( ωt 2 ) sin(h + ωt 2 ) v t+t = v h t+t = h + ωt ω t+t = ω

31 Sensor Fusion, 2014 Lecture 9: 31 Sounding rocket Navigation grade IMU gives accurate dead-reckoning, but drift may cause return at bad places. GPS is restricted for high speeds and high accelerations. Fusion of IMU and GPS when pseudo-ranges are available, with IMU support to tracking loops inside GPS. Loose integration: direct fusion approach y k = p k + e k. Tight integration: TDOA fusion approach y i k = p k p i k /c + t k + e k.

32 Sensor Fusion, 2014 Lecture 9: 32 MC leaning angle Headlight steering, ABS and anti-spin systems require leaning angle. Gyro very expensive for this application. Combination of accelerometers investigated, lateral and downward acc worked fine in EKF. Model, where z y, z z, a 1, a 2, J are constants relating to geometry and inertias of the motorcycle, u = v x x = (ϕ, ϕ, ϕ, ψ, ψ, δ ay, δ az, δ ϕ ) T, y = h(x) = ay ux 4 z y x 3 + z y x4 2 tan(x ( 1) + g sin(x 1 ) + x 6 a ż = ux 4 tan(x 1 ) z z x x4 2 tan2 (x 1 ) ) + g cos(x 1 ) + x 7 ϕ a 1 x 3 + a 2 x4 2 tan(x 1) ux 4 J + x 6

33 Sensor Fusion, 2014 Lecture 9: 33 Virtual yaw rate sensor Yaw rate subject to bias, orientation error increases linearly in time. Wheel speeds also give a gyro, where the orientation error grows linearly in distance, Model, with state vector x k = ( ψ k, ψ k, b k, r ) k,3 r k,4, y 1 k = ψ k + b k + e 1 k, y 2 k = ω 3r nom + ω 4 r nom 2 2 B ω 3 ω 4 r k,3 r k,4 1 ω 3 ω 4 r k,3 r k, e2 k.

34 Sensor Fusion, 2014 Lecture 9: 34 Friction estimation Friction model y k = 1/sµ k + δ k + e k, where y k is the slip, s is the friction dependent slip slope, µ k is the normalized traction force from the engine, δ k a tire radius dependent offset and e k is measurement noise. Kalman filter estimated s adaptively. When s decreases under a threshold, a friction alarm is issued.

35 Sensor Fusion, 2014 Lecture 9: 35 Tire Pressure Monitoring System TPMS required in US since 2007, and in EU from Direct systems (with pressure sensors inside tire) most common, but indirect systems based on wheel speeds (ABS sensors) also on the market. The TPMS by NIRA Dynamics is installed in 4.75 (may 2013) million cars (Audi, VW, Skoda, Seat). Based on many different physical principles and EKF s, including the two ones above!

36 Sensor Fusion, 2014 Lecture 9: 36 Shooter localization Network with 10 microphones around a camp. Shooter aiming inside the camp. Supersonic bullet injects a shock wave followed by an acoustic muzzle blast. Fusion of time differences give shooter position and aiming point. Model, with x being the position of the shooter, y MB k = t 0 + b k + 1 c p k x + e MB k, y SW k = t 0 + b k + 1 r log v 0 v 0 r d k x + 1 c d k p k + e SW k. where d k is an implicit function of the state.

37 Sensor Fusion, 2014 Lecture 9: 37 Shooter localization results WLS loss function illustrates the information in yk MB yk SW, k = 1, 2,..., N = 10. Both shooter position and aiming direction α well estimated for each shot. Also bullet s muzzle speed and ammunition length can be estimated!