Kalman Filtering Pieter Abbeel UC Berkeley EECS - PowerPoint PPT Presentation

Kalman ¡Filtering ¡ ¡ ¡ Pieter ¡Abbeel ¡ UC ¡Berkeley ¡EECS ¡ ¡ ¡ ¡ ¡ Many ¡slides ¡adapted ¡from ¡Thrun, ¡Burgard ¡and ¡Fox, ¡ProbabilisAc ¡RoboAcs ¡ ¡ ¡

Overview ¡ Kalman ¡Filter ¡= ¡special ¡case ¡of ¡a ¡Bayes’ ¡filter ¡with ¡dynamics ¡model ¡and ¡ n sensory ¡model ¡being ¡linear ¡Gaussian: ¡ 2 -1 Above ¡can ¡also ¡be ¡wriLen ¡as ¡follows: ¡ ¡ n Note: I switched time indexing on u to be in line with typical control community conventions (which is different from the probabilistic robotics book).

Time ¡update ¡ Assume ¡we ¡have ¡current ¡belief ¡for ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡: ¡ n X t X t+1 Then, ¡aNer ¡one ¡Ame ¡step ¡passes: ¡ n

Time ¡Update: ¡Finding ¡the ¡joint ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ n Now ¡we ¡can ¡choose ¡to ¡conAnue ¡by ¡either ¡of ¡ ¡ n (i) ¡mold ¡it ¡into ¡a ¡standard ¡mulAvariate ¡Gaussian ¡format ¡so ¡we ¡can ¡read ¡of ¡ the ¡joint ¡distribuAon’s ¡mean ¡and ¡covariance ¡ n (ii) ¡observe ¡this ¡is ¡a ¡quadraAc ¡form ¡in ¡x_{t} ¡and ¡x_{t+1} ¡in ¡the ¡exponent; ¡the ¡ exponent ¡is ¡the ¡only ¡place ¡they ¡appear; ¡hence ¡we ¡know ¡this ¡is ¡a ¡ mulAvariate ¡Gaussian. ¡ ¡We ¡directly ¡compute ¡its ¡mean ¡and ¡covariance. ¡ ¡ [usually ¡simpler!] ¡

Time ¡Update: ¡Finding ¡the ¡joint ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ n We ¡follow ¡(ii) ¡and ¡find ¡the ¡means ¡and ¡covariance ¡matrices ¡in ¡ [Exercise: Try to prove each of these without referring to this slide!]

Time ¡Update ¡Recap ¡ Assume ¡we ¡have ¡ n X t X t+1 Then ¡we ¡have ¡ n Marginalizing ¡the ¡joint, ¡we ¡immediately ¡get ¡ n

Generality! ¡ Assume ¡we ¡have ¡ n V W Then ¡we ¡have ¡ n Marginalizing ¡the ¡joint, ¡we ¡immediately ¡get ¡ n

ObservaAon ¡update ¡ X t+1 Assume ¡we ¡have: ¡ n Z t+1 Then: ¡ n And, ¡by ¡condiAoning ¡on ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡(see ¡lecture ¡slides ¡on ¡Gaussians) ¡we ¡readily ¡get: ¡ n

Complete ¡Kalman ¡Filtering ¡Algorithm ¡ At time 0: n For t = 1, 2, … n n Dynamics update: n Measurement update: n Often written as: (Kalman gain) “innovation”

Kalman ¡Filter ¡Summary ¡ n Highly ¡efficient: ¡Polynomial ¡in ¡measurement ¡dimensionality ¡ k ¡ and ¡state ¡dimensionality ¡ n : ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ O(k 2.376 ¡+ ¡n 2 ) ¡ ¡ n OpAmal ¡for ¡linear ¡Gaussian ¡systems! ¡

Forthcoming ¡Extensions ¡ Nonlinear ¡systems ¡ n Extended ¡Kalman ¡Filter, ¡Unscented ¡Kalman ¡Filter ¡ n Very ¡large ¡systems ¡with ¡sparsity ¡structure ¡ n Sparse ¡InformaAon ¡Filter ¡ n Very ¡large ¡systems ¡with ¡low-­‑rank ¡structure ¡ ¡ ¡ n Ensemble ¡Kalman ¡Filter ¡ n Kalman ¡filtering ¡over ¡SE(3) ¡ n How ¡to ¡esAmate ¡ A t , ¡ B t , ¡ C t , ¡ Q t , ¡ R t ¡from ¡data ¡( z 0:T , ¡ u 0:T ) ¡ n EM ¡algorithm ¡ n How ¡to ¡compute ¡ ¡ ¡ ¡(= ¡smoothing) ¡ ¡(note ¡the ¡capital ¡“T”) ¡ n

Things ¡to ¡be ¡aware ¡of ¡that ¡we ¡won’t ¡cover ¡ Square-­‑root ¡Kalman ¡filter ¡-­‑-­‑-­‑ ¡keeps ¡track ¡of ¡square ¡root ¡of ¡covariance ¡matrices ¡-­‑-­‑-­‑ ¡equally ¡fast, ¡numerically ¡ n more ¡stable ¡(bit ¡more ¡complicated ¡conceptually) ¡ If ¡ A t ¡= ¡A, ¡ Q t ¡= ¡Q, ¡ C t ¡= ¡C, ¡ R t ¡= ¡R ¡ ¡ ¡ n If ¡system ¡is ¡“observable” ¡then ¡covariances ¡and ¡Kalman ¡gain ¡will ¡converge ¡to ¡steady-­‑state ¡values ¡for ¡t ¡-­‑> ¡ 1 n Can ¡take ¡advantage ¡of ¡this: ¡pre-­‑compute ¡them, ¡only ¡track ¡the ¡mean, ¡which ¡is ¡done ¡by ¡mulAplying ¡Kalman ¡gain ¡with ¡ n “innovaAon” ¡ System ¡is ¡observable ¡if ¡and ¡only ¡if ¡the ¡following ¡holds ¡true: ¡ ¡if ¡there ¡were ¡zero ¡noise ¡you ¡could ¡determine ¡ n the ¡iniAal ¡state ¡aNer ¡a ¡finite ¡number ¡of ¡Ame ¡steps ¡ Observable ¡if ¡and ¡only ¡if: ¡ ¡rank( ¡[ ¡C ¡; ¡CA ¡; ¡ CA 2 ¡; ¡ CA 3 ¡; ¡… ¡; ¡ CA n-1 ]) ¡= ¡n ¡ n Typically ¡if ¡a ¡system ¡is ¡not ¡observable ¡you ¡will ¡want ¡to ¡add ¡a ¡sensor ¡to ¡make ¡it ¡observable ¡ n Kalman ¡filter ¡can ¡also ¡be ¡derived ¡as ¡the ¡(recursively ¡computed) ¡least-­‑squares ¡soluAons ¡to ¡a ¡(growing) ¡set ¡of ¡ n linear ¡equaAons ¡

Kalman ¡filter ¡property ¡ If ¡system ¡is ¡observable ¡(=dual ¡of ¡controllable!) ¡then ¡Kalman ¡filter ¡will ¡converge ¡to ¡the ¡true ¡state. ¡ n System ¡is ¡observable ¡if ¡and ¡only ¡if: ¡ n ¡O ¡= ¡[C ¡; ¡CA ¡; ¡ CA 2 ¡; ¡… ¡; ¡ CA n-1 ] ¡ ¡ ¡is ¡full ¡column ¡rank ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡ ¡(1) ¡ IntuiAon: ¡if ¡no ¡noise, ¡we ¡observe ¡y 0 , ¡y 1 , ¡… ¡and ¡we ¡have ¡that ¡the ¡unknown ¡iniAal ¡state ¡x 0 ¡saAsfies: ¡ ¡y 0 ¡= ¡C ¡x 0 ¡ ¡y 1 ¡ = ¡CA ¡x 0 ¡ ¡... ¡ ¡y K ¡= ¡CA K ¡x 0 ¡ This ¡system ¡of ¡equaAons ¡has ¡a ¡unique ¡soluAon ¡x 0 ¡iff ¡ ¡the ¡matrix ¡[C; ¡CA; ¡… ¡ CA K ] ¡has ¡full ¡column ¡rank. ¡ ¡B/c ¡any ¡power ¡of ¡ a ¡matrix ¡higher ¡than ¡n ¡can ¡be ¡wriLen ¡in ¡terms ¡of ¡lower ¡powers ¡of ¡the ¡same ¡matrix, ¡condiAon ¡(1) ¡is ¡sufficient ¡to ¡ check ¡(i.e., ¡the ¡column ¡rank ¡will ¡not ¡grow ¡anymore ¡aNer ¡having ¡reached ¡K=n-­‑1). ¡