Adaptive Kalman Filter

by allenlu2007

 本文主要 focus on noise unknown and time varying signal or noise 時的 Kalman filter.

NewImage

其實 Kalman filter 理論上很漂亮。實用上最大限制是 noise 的 covariance matrix (Q and R) 必須是已知 (上圖的 Σ 或下圖的 R)。

同時在算 Kalman filter recursive equation 非常奇怪的一點就是Kg(k) 和 P(k) (kalman gain and estimation error covariance matrix) 居然和 observation (Z(k)) 無關!!  特別是當 Q=0 (no process noise) 只有 observation noise R. 通常 noise covariance 很難事先知道。感覺上有點運氣的成份

Another problem is if the Q and R are time varying (unknown prior), then conventional Kalman filter may has lots of error, or even diverge!!!

Solution:

1. 用 brute force Monte Carlo (MCMC) simulation.  不過少了 physical insight.

2. 用 EM algorithm 來找 maximum likelihood 的 noise variance (and A, H)

3. Adaptive Kalman Filter (using variational Bayesian or other methods)

 

We focus on (3) because it can deal with time varying situation!!

 

我們主要先考慮 VB inference 因為可以把 noise variance 和 observations 連結一起。

目標是計算 posterior distribution:

NewImage

下圖的 μ 為 x(k),  Φ 為 Σ(k),  x 為 y(k)

NewImage

Initialisation and prediction 基本上都和 Kalman filter 一致。困難的是 eq. (3) 的 integration.

密訣是假設 mean 是 normal pdf;  variance 是 Gamma distribution 同時 mean and variance 是 independent.

NewImage 

By Bayesian directed graph rule, 上述的 conditional mean and variance 非 independent.  

我們仍用這樣的近似。

NewImage

NewImage 

NewImage 

NewImage 

注意以上的 equations 是聯立方程式 (mk, Pk, Σk are unknowns 但有三個 equations to solve).

為了避免解聯立方程式,我們再把每個 update phase 做 N 次 iteration 來解如下。

Predict phase 和之前 equation 相同,不過引入了 noise 的 heuristic dynamic parmeter ρ. 在 time varying noise 時 ρ 的角色很重要,更詳細的說明請見後文。 

Update phase 就是用 fixed N iteration 來解上述 equation (7). 

NewImage 

ρ = 1 : stationary noise variance.  ρ < 1 表示 precision Φ (1/σ^2) 的 variance 增加。

NewImage 

  

Example 2D.1:  DC + stationary noise with unknown variance (R)

上述公式可以簡化如下 (No process noise, i.e. Q=0)

NewImage

乍看之下, noise variance estimation Σ 和 solution (2A)-(2C) 有點像但又不同。

Σ ~ 1/n {sum (yk- mk)^2} + 1/n {P1 + P2 + … + Pn}

第一部份和 empirical variance 相似但不同。重點是 mk 是 local mean 而非 empirical mean.

m1, m2 和 empirical mean 可能差異比較大,但到 mn (n is large) 時, mn 就很接近 empirical mean.

直覺來說,當 n 很小時,Kalman gain 接近 1, (yk-mk) 比較小,

因此需要第二部份 Pk (=var(xk-mk)) 來補償。

當 n 夠大時,Kalman gain 接近 0, (yk-mk) 比較接近 empirical variance, 這時 Pk 接近 0.

 

因為 Q=0, 嚴格來說 Kalman filter 並不會收歛。但等到 k 夠大時,基本上接近 empirical variance.

以下是簡單的推導,說明 ( 非証明 ) equation (7) 可以 estimation noise variance.

NewImage

 

接下來考慮一個更複雜的 case,  Q != 0.  這是原來問題的推廣。除了 sensor noise 之外,

還包含了 process noise.  原來的 2A-2C 無法包含 process noise. 

NewImage

只是 Q =0 的推廣。比較重要的是 steady state 的 mean and variance summarized 如下。

NewImage

我們假設 noise variance R unknown, 重點是如何得到 R.  Again,  equation (7) 的 Σk 就是 R 的 estimation.   當然 equation (7) 在 steady state 也必須是正確。以下先是直覺的說明。

我們分為兩種極端的 case.  First R >> Q.  Q=0 簡化為前例。在 steady state, Kalman gain -> 0;         mk -> empirical mean;  P+ -> 0;  R -> empirical variance 正如我們預期。

反之 Q >> R.  此時 Kalman gain -> 1;  mk -> yk;  P+ -> R; 或者說 R -> P+.

NewImage

 當然介於兩種極端 case 之間的 steady state 証明如下,先用錯誤的証明:

NewImage

 正確的証明有兩個,簡單的和複雜的。

NewImage

 NewImage

可以看到引入 HMM 比 plate model 更 general.   (1) 是 recursive method 而非 batch method (需要等到所有的 measurements).  Recursive method 非常適合 real time processing.   (2) 更 general 可以加上 process noise (Q > 0), dynamic model (A != 1).  以下是 simulation results.

Case A:  Q=0 or R >> Q   (mean=-3.8; Q=1e-5; R=0.81)  

下圖是 mean 的 true value (state), measurements, estimation. 和普通 Kalman filter 不同的是 noise variance R is unknown. 是由 equation (7) estimate.  

 NewImage

下圖是 noise variance R 的 variance 以及 break down to empirical variance and P (residue variance).

前文解釋過: 開始時 K 接近 1, estimate mean 接近 measurement, empirical variance 非常小。

residue variance (P) ~ R/n.  這由 log plot 可以看出。

隨著 estimate mean 接近 true mean, K 接近 0, empirical variance dominate.  Residue variance -> 0. 

NewImage

Log scale. 

NewImage

 

Case B:  Q!=0  (mean=-3.8; Q=0.05; R=0.81)  

 NewImage

linear scale.

NewImage

log scale.

NewImage 

 

Example 2D.2:  DC + time varying noise with unknown variance (R)

(python code: ~/work/python/kalman/dc_var/dc_var_time_varying.py)

HMM 除了 recursive 和更 general 的好處外。另一個非常實用的好處是可以引入 time varying 的觀念,

例如 time varying noise estimation.

以下的說明完全錯誤,正確見後文:

對於 time varying noise 來說,ρ 是一個非常重要的參數。 ρ 可以視為遺忘 factor, 把之前 estimated precision 遺忘掉。ρ=1 是所有的 (αk, βk) equally weight, 比較適合在 stationary noise variance.  ρ<1 表示目前的 measurement 的 weight 比較重,等效遺忘之前的 measurement.

下圖是 DC value (-3.78) and Q=0 加上 time varying noise.  前 1/3 variance R = 0.81, 中 1/3 variance 10*R, 後 1/3 variance 5*R.   

 NewImage

下圖是不同 ρ 時 equation (7) 的 R estimation 結果。

First ρ = 1 

NewImage

可以看到雖然 estimated R 有 adaptive to true R, 但反應的速度太慢。這個問題可以改變 ρ 解決。

下圖是 ρ = 0.995

NewImage

基本上可以 adapt to time varying noise variance. 

下圖是 ρ = 0.99

NewImage

下圖是 ρ = 0.95.  可以結論如果 ρ 愈小,反應速度愈快但是愈 noisy.  似乎 ρ ~ 0.99 是合理的 trade-off.

NewImage 

下圖是 ρ = 1.01. 完全無法 track noise variance 的變化。

NewImage 

實務上應讓 ρ ~ 0.99 for time varying noise system.  Simo 是用 1-exp(-4)=0.98. 

 

再解釋 ρ 的角色: 之前提到 ρ 是遺忘 factor 是錯誤的說法。

應該從 precision Φ (=1/R) 的 pdf 為 Gamma (α, β) 來看:  

E(Φ) = α / β;   Var(Φ) = α / β^2

在 predict phase, α -> ρα and β -> ρβ    E(Φ) 不變,意即 precision (=1/R) 的平均值仍然不變。  

但是 Var(Φ) -> α / (ρβ)   如果 ρ < 1 代表 precision 的 variance (time fluctuation) 變大。

Φ (=1/R) 的平均值不變代表從 time step k-1 to k 的 predict phase,  R 可以平滑過渡。

variance 變大代表 fluctuation 變大,在 update phase R 比較快 adapt to new measurement, 但 trade-off 就是 R 的fluctuation 變大。

也可以直接從 R=σ^2 的 pdf 為 Inverse Gamma 來看:

E(R) = β / (α-1) ~ β/α    Var(R) = β^2 / (α-1)^2(α-2) ~ β^2/α^3   when α >> 1

也會得到同樣的結論: R 的 variance (fluctuation) 變大。

 

重點是:

Kalman filter:  R 是 constant. 可以是 prior known time varying constant, 但必須是 constant.

Noise adaptive Kalman filter 則是把 R 當作 random variable, 用 Bayesian inference or estimate.  另外加上 ρ factor 讓 adaptive speed 變快

在某一些特別需要準確 estimate noise variance 的情況,如 SNR estimation.  顯然一定需要準確快速的 noise adaptive Kalman filter.

另外在一些只 care state 的 estimation (i.e. Gaussian pdf 的 mean), R 的準確與否似乎差異不大。因為只要 R >> Q, 很快 Kalman gain K –> 0,  state 也會收歙到正確的 DC 值,即使 R 猜錯了或 R 有大的 estimation error.   DC (or state) estimation error ( true state – estimated state) 基本上不隨 R 的 error 有任何的差異。(Again, 因為 Kalman gain 已經非常接近 0, estimation error 也非常接近 0).  以上述的三段式為例:

Q = 0

ρ = 1.01  –>  rms error: (0.000569, 0.000063, 0.000020)

ρ = 1    –>  rms error: (0.001061, 0.000022, 0.000003)

ρ = 0.99    –>  rms error: (0.001084, 0.000008, 0.000004)

如果 R 沒有遠大於 Q 時, K 不會收歛到 0 且 depends on R and Q.  在這種情況下 R 的 estimation 就可能影響最後的 steady state error 以及收歛的速度。下面舉一些例子。

Q = 0.05 情況就不同。此時因為 K 比 0 大很多 ( 0 < K < 1),R estimation 的準確就會對最後 state estimation error 有相當的影響。

ρ = 1.01  –>  state rms error: (0.175, 1.015, 0.518)

NewImage

Kalman gain 完全不 adapt

NewImage

R estimation 非常不準

NewImage

State error 因此不好,在第二段的 state error 變大很多因為 Kalman gain 太大,所以 measurement 所佔的比例增加而造成 state error 變大。

NewImage

 

以下是 ρ = 1  –>  state rms error: (0.177, 0.710, 0.356)

 Kalman gain 有一些 adaptive

NewImage

R estimation 比較準

NewImage

State error 有些改善但仍不理想

NewImage

 

以下是 ρ = 0.99  –>  state rms error: (0.170, 0.500, 0.382)

Kalman gain adapt to time varying noise

NewImage

R estimation 也有 adapt

NewImage

State error 也更有改善

NewImage

 

Example 2D.3:  Time varying unknown signal (Q) time varying noise with unknown variance (R)

更進一步是 estimated signal 也是 time varying 的 case.  可以分為幾種 cases

A. 如上述的例子,Q 為已知或0 (if 0, the signal is stationary of course)

B. 如上述的例子,但 Q 為未知

C. piecewise DC, with known or unknown transition time stamp.  Q is dynamic.  Without transition, Q is small; with transition Q is large.  如果 known time stamp, Q –> qk where qk is known.

如果 unknown time stamp, 可以用另一個 HMM 來 model Q!!  同樣用 PGM 可以 inference 所有的 information.

 

Case A: 上文已經討論過。除了 Q=0 的 case, 實務上很少 Q 為 known prior information.

Case B: Q 是未知但為 constant.  可以用 EM algorithm 來 estimate?  還是 try-and-error? 多半是把 Q 當作 tune parameter.  逐漸加大 Q 直到 estimated error 最小。參見 “Kalman Filter Example: Water Tank”  有時候加大 Q 不一定是最好的方法。

比較好的方法是改變 state 的定義 (如增加 state 的 dimension – spatial or time dimension) ,更準確的 estimate state 而減小 Q (or even 0), 而不需要加大 Q.  以下為例:

(a) estimate moving objective position from k to k+1.  It’s better to add velocity into state variable.  In a sense, we add k-1 into the state.   ( position(k-1), position(k))  or (position(k), velocity(k)) 

(b) estimate 1st order filter (RC) step response.  The same idea as above (reference …)

簡單結論: 與其加大 Q (uncertain), 不如找比較好的 dynamic model 來描述 states (make it more certain)

Bottom line: 大部份 well model 的 dynamic system,  R >> Q.   但然有例外,因為某些 system 的 state uncertainty 真的比較高 ( 如 navigation).  

有很多的 paper and algorithm (adaptive) 討論如何做 adaptive Kalman filter with unknown process (and measurement noise).   Quote some references 如下。當然 computation 的複雜度就更高。

Myers “adaptive sequential estimation with unknown noise statistics” based on correlation matching method

Sage/Husa “adaptive filtering with unknown prior statistics” based on Bayesian approach

 

(c) 我比較有興趣是這一類的 case, 在 communication system 常發生。在 RX dc offset estimation or SNR estimation. 如果是 RX 的 gain or path 調整,time stamp 為已知。調整時 both dc and noise 跟著一起改變。  如果是 TX 的 symbol 調整 (e.g. baseline wandering), time stamp 未知,調整的 baseline (dc) changes, 但 noise 不變。

如何處理已知 time stamp time varying, 有兩種方式: 一種是加大 process noise (Q); 另一種是改善 state estimation 更精確 capture time varying behavior. 

First method, 只要在 time stamp 時 Q –>  Q_large; after some time (one or two or more time step) Q –> Q_small.

對應的結果如下圖。先看 Kalman filter estimation 時 Q 不變的情況做為比較。

NewImage

zoom in 可以看出,如果 Q 不改變,Kalman filter 不能 adapt to DC offset change. 

NewImage

 

以下是 Q 在已知 time stamp switch 到 Q_large; 其他時間都是 Q_small. 

NewImage

Zoom in 如下圖。可以看到 Q 變大雖然能快速 adapt,但有機率調大或調錯會讓 error 變大。

NewImage

 

最好的方法是 Q match changes?

多階式的 Q –> Q_large and gradually Q_small 會更好?

另一問題是 Q_large 時間多長?  Q_large 應該多大?  太多參數要調非好的方法。

似乎和每次重新 reset 一樣,之前的 statistics 看來可以放棄。

 

Second method, 改善 state estimation.  Fundamentally, 與其 fit Q, 不如 fit state model.

Method 1: 是用一階 differential equation (RC or LR filter) 來逼近 transition response 如下圖, 我們應用 q and q_dot (q is charge, for RC case);  or i and i_dot (for LR case).  如果一階 response 可以貼近 true state, Q 可以等於 0 或是非常小。
 
但問題是  x(k+1) = A x(k) + B u(k) + q(k) 中的 A 和 B 一般而言是未知。需要 estimate.

另一個缺點是 transition response 如果不是一階而是二階有 damped oscillation.  一階 differential equation 無法非常貼近 true state。

 

NewImage

 

另一個方式是用 constant velocity model (如 water tank example) with variable process noise Q.

特別 process noise  

 

estimate x(k) and x_dot(k) 但假設 constant velocity model with uncertain Q.

x(k+1)   =   x(k) +  x_dot(k)   +   q(k)

x_dot(k+1) =  x_dot(k)  + q_dot(k)

可以設 q(k) = 0;  只控制速度的 process noise: q_dot(k)  

y(k) = x(k) + r(k);  只 estimate position noise: r(k)

        

先用一個例子:  constant R (=10), time varying velocity

我們用 EM algorithm with assumed q_dot(k)=1e-4 (fixed, not time varying) but unknown r(k) to estimate state and noise.   Kalman filter and Kalman smoother estimation of position and velocity 如下圖.  

NewImage

Zoom in to 轉折點如下圖.  Kalman smoother fit 比較接近,Kalman filter 因為 real time (only based on 之前的 measurements) 會 lag.  

zoom in, 大約 25 個 samples 才追上。Kalman smoother 則沒有 lag 因為 estimate 是 based on 所有 (包含之前和之後) 的 measurements.

NewImage

velocity state

NewImage

Estimation errors (both position and velocity; Kalman filter and Kalman smoother) 如下圖。

如所預期,estimation errors 最大是在轉折點時。Kalman filter 的 error 大於 Kalman smoother.

Estimated RMS Noise level:  

1st position rms error_filter: 0.751502
2nd position rms error_filter: 2.033705
3rd position rms error_filter: 2.166452
1st position rms error_smooth: 0.845816
2nd position rms error_smooth: 1.230508
3rd position rms error_smooth: 0.863940

tbNewImage 

再看 Kalman gain 的變化如下圖。基本上是 fixed value.  為什麼開始 Kalman gain 變大 TBC.

NewImage

 

(Kalman q_dot = 1e-2)

NewImage

zoom in, adapt 的速度似乎快了幾個 samples.

NewImage

velocity state 也快一點,但是 noise 也增加不少。

NewImage

(Kalman q_dot = 1e-6)

NewImage

 NewImage

可以看出反應速度太慢。實際上看應用而訂,trade-off between response speed and noise!

NewImage

假設 observation noise 是 time varying R = (1, 10, 5)

NewImage

第一部份 rms error 比較小,之後都差不多。

1st position rms error_filter: 0.261825
2nd position rms error_filter: 1.231322
3rd position rms error_filter: 1.952864

1st position rms error_smooth: 0.468104
2nd position rms error_smooth: 0.900828
3rd position rms error_smooth: 0.570492

NewImage

Kalman gain 比較大一點,但都是 constant.

NewImage

In summary, constant velocity 的好處

a. 使用一個 q_dot parameter 就可以 fit time varying input signal.  這還是假設未知 time varying 時間的 case (blind adaptive).  (python program: ~/work/python/kalman/velocity/sig_var_time_varying2.py) 

缺點是 fixed q_dot 會造成 response lag, 以及 rms noise level 比較大。

如果已知 time varying time stamp, 應可以用更精碓的方法。不過 pykalman 似乎無法處理 time varying covariance 的 case.  暫時無法提供 time varying q_dot 的 simulation 結果。方法是 transition 時 q_dot 設為 1.0 or larger; 但在 transition 後幾個 time steps 後把 q_dot 設為 1e-6 or smaller.   具體的 q_dot 還是需要 fine tune.  

我 improve 之前的 variational Bayesian python code to cover constant velocity case with q_dot. (python program: ~/work/python/kalman/velocity/sig_var_time_varying4.py)  基本上 q (position) = 0;  q_dot (velocity) 和多少 time samples 是 tuning parameter, 另一個 tuning parameter rho, 控制 noise R adaptive 的速度。

 

下圖為 dynamic q_dot 的結果。(q_dot large=1, q_dot small=1e-10), t=3 是轉折點 q_dot large 有三個 samples.  其他都是 q_dot small.    比起之前 fixed q_dot, 反應時間快很多 (controlled by q_dot large and t),同時 error 小很多 (controlled by q_dot small).  這和預期一致。 

NewImage

 

NewImage

 

velocity 的部份更明顯,response time and error 都遠比 fixed q_dot 好。

NewImage

 

可以從 Kalman gain 的變化得到更多的 information.  在轉折點的 q_dot large, Kalman gain 變大,measurements 比較 reliable.  之後 q_dot small, Kalman gain approach 0, prediction 變成 reliable.

NewImage

 

Estimated R (1, 10, 5) 仍然有比較大的誤差。不過 Kalman filter 似乎對 R estimation error 有一定的 tolerance.

NewImage

position and velocity errors 如下。

1st position rms error: 0.117384
2nd position rms error: 0.567931
3rd position rms error: 0.572720
1st velocity rms error: 0.004345
2nd velocity rms error: 0.079732
3rd velocity rms error: 0.031844

NewImage

如果 rho 變小,R 的 adaption 變快。rho=0.95 如下:

NewImage

NewImage

 

NewImage

NewImage

 

NewImage

NewImage

 

再考慮 R = (10, 10, 10) 的 case:

turn sample = 3;  rho = 0.99

NewImage

NewImage

NewImage

NewImage

 

** close form for q_dot noise and r noise!!

 Example2:  PLL!!! seems to be the same thing using constant velocity model!!

 Example3: baseline wandering  

 

Advertisements