Kalman Filter and PID Controller

by allenlu2007

PID Controller

PID controller 如下圖。 y(t) 和 u(t) 是 plant/process 的 output and input.

r(t) 是 desired or reference signal, or signal to be tracked.   Error signal e(t)=r(t)-y(t) 經過

PID controller 再 feedback 回 plant/process 的 input.  

在穩態 (DC) 時 loop gain (PID * plant/process) 很大 (due to Integrator): e(t)=0

y(t) = r(t) 意即 plant/process output tracks r(t). 

一個常見的 special case 是 r(t)=0

NewImage

 

一個重要但 stupid 的問題: 為什麼需要用 y(t) to duplicate r(t) 而不直接用 r(t)? 

有幾個原因:

1. 傳統 control system plant 是 actuator or high power driver.  r(t) 並不能直接 drive output load, 需要 y(t) 來 drive output.

2. 傳統電子電路的 opamp (類似 I: integrator controller).  Plant/process 包含一個精確的 attenuator (電阻分壓), 因此 u(t) * β = y(t) = r(t)  (β < 1) => u(t) = r(t)/β 可以精確放大 r(t)  1/β 倍。另一個例子是 PLL based frequency synthesizer (見後文). 同樣 VCO 類似 I: integrator controller.  1/N 是精確 divider.  因此 u(t) 可以將 frequency 放大 N 倍。

其實 1, 2 類都是類似的應用: 放大功率、電壓或電流、頻率。注意 loop gain (plant * PID controller) 一定要有 integrator 才能讓 error e(t) 在穩態時趨近 0.

3. r(t) 可能包含 signal + noise.  如果適當調整 PI controller 等效改變 loop bandwidth, 可以讓 y(t) 的 SNR 比 r(t) 的 SNR 更好。 SNR_y > SNR_r.  最常見的例子是 PLL or CDR.   另外在一些 dc or frequency offset estimation 或 timing recovery 也有類似的應用。

第 3 類的特性就是 filter, 這和 1, 2 類應用相反。在1, 2 類應用 plant 或 PID controller 都可能加入 noise 到 y(t). 通常 SNR_y < SNR_r.   但在第 3 類的應用可以讓 SNR_y > SNR_r.

嚴格來說,古典的 control theory (PID controller, etc.) 並不處理 noise issue.  Actually, D 反而是 noise amplifier.  一般是放在 stochastic signal processing 的 context 中討論。Kalman filter 就是其中的一個 model/solution.  不過在類比電路信號處理,常常用 PI filter (或更高階 integrator filter) 同時處理 tracking 和 filtering 問題。PI 並不是 optimal filter, 但是簡單明白,適合類比信號處理。請見後文。 

 

Why PID Controller

I (integrator) 提供最重要的 high DC (or low frequency) gain, 才能讓 e(t)->0

P (proportional) 提供同樣重要的 fast response (因為 integrator 要積分一段時間才生效,或是 integrator phase lag 90度)。對於一些 plant/process 本身已經包含 integrator (1/s).  P 在高頻提供 positive phase shift (zero) 而避免 overshoot 甚至 oscillation.

D (differential) 提供更快的 response (微分是反應 local 的特性)。但 D 會放大 noise (特別是高頻的 noise), 另外多了 D, PID controller 變成 2 階的系統,似乎穩定性也容易有問題。容易產生 overshoot 或 oscillation.

Summary:

1. 大多數的應用只用 PI controller 就可以。即使在 PID 系統中的 Kd 一般也遠小於 Ki and Kp, use it with caution.

2. 大多實際應用的 plant 包含很多 unknown factors (noise, inaccuracy, friction)。因此 Ki, Kp, Kd 雖然有 rule-of-thumb 但多半是 try-and-error 試出 comprised solution. 

 

PID Example1: 倒單擺車

Control 的古典範例是倒單擺車如下圖。Two observations (measurements) 包括 angle (θ) 和 position (P)

One control (input) 是 Force (or velocity) υ.  穩態的 solution:  θ~0;  P depends, 見下圖 matlab model.

NewImage

下圖是倒單擺車的 Matlab model。

y(t) 包含了 two outputs (position and angle) and one input (force).  Desired angle = 0, 因此直接 feedback to PID controller 然後再回 input.  Desired position 則可能是 random control 或是 joystick 定點或 control.  Position error feedback to PID controller 然後再回 input.  

以上 Matlab model 並不含 noise.  事實上不論 angle 或 position 的 measurement 都會有 noise. 更麻煩的是 plant/process 包含非常多的 process variations 或不確定的因子,例如 motor variation, friction, etc.   如前所述,D 是 noise amplifier.  因此 PID controller 用在倒單擺車,特別是 cheesy 倒單擺車會有些挑戰。

Solutions:  (A) PI controller; (B) Robust control theory; (C) Stochastic signal processing (e.g. Kalman filter);  (C) Machine learning.

(A) 是偷吃步,通常用 try-and-error solution; (B) 對我太難; (C) 本文討論 PID controller 和 Kalman filter 的相似和相異點。以及 trade-off;  (D) Machine learning 和 Kalman filter 可以放在更 general framework, PGM (probabilistic graph model) 之內 (參考 Stanford professors: Andrew Ng and Daphne Koller 的 lectures).

NewImage

 

PID Example2: Phase lock loop (PLL)

PLL 也是一個古典的電路控制範例。error signal 經過 CP+loop filter, 一般是 PI filter (2nd order filter) 或 PII filter (3rd order filter).  Plant/process 就是 VCO, a phase integrator.  Feedback divider 的作用是放大 frequency. 

NewImage

Noise 在 PLL 是 key issue.  Rule of thumb: 如果 dominant noise 是在 θref, loop filter 變小會比較好。 如果 dominant noise 是在 VCO (additive before the Kv/s), loop filter 變大會比較好。 如果要 fast acqusition, loop bandwidth 變大會比較好。

以上都是概略的做法,Kalman filter 可以用來處理這方面的問題。因為 Kalman filter 本身就是 stochastic signal based estimation. 除了一些 Gaussian noise 的假設,沒有其他的假設。前文提到 Kalman filter based PLL 基本形式和 PI controller 一樣,不過是 time-varying PI controller.  

Kalman Filter as Feedback Loop

The Kalman Filter

For a linear Gaussian process with input u, output z and internal state, x

$$ x_k = Ax_{k-1} + Bu_{k-1} + \omega_{k-1}, \,\, \omega_{k-1} \sim N(0,Q)$$

$$ z_k = Cx_k + Du_k + \nu_k, \,\, \nu_k \sim N(0,R)$$

The Kalman filter is normally represented in two stages: a predict stage,

$$\mbox{(1) predict state: }\hat x_{k|k-1} = A\hat x_{k-1|k-1} + Bu_{k-1}$$

$$\mbox{(2) predict covariance: }P_{k|k-1} = AP_{k-1|k-1}A^T + Q$$

and a update stage

$$\mbox{(3) Innovation: }\tilde y_k = z_k  - C\hat x_{k|k-1} - Du_k$$

$$\mbox{(4) Innovation Covariance: }S_k = CP_{k|k-1}C^T+R$$

$$\mbox{(5) Kalman gain: }K_k=P_{k|k-1}C^TS_k^{-1}$$

$$\mbox{(6) update state: }\hat x_{k|k}=\hat x_{k|k-1} + K_k\tilde y_k$$

$$\mbox{(7) update covariance: }P_{k|k}=(I-K_kA)P_{k|k-1}$$

A Feedback View of the Kalman Filter

However, to view the Kalman filter as a feedback control system may provide more insight to understand its principle. Re-arrange the seven equations of the Kalman filter described above into the following groups:

Process model group:

$$\mbox{(1) predict state: }\hat x_{k|k-1} = A\hat x_{k-1|k-1} + Bu_{k-1}$$

Kalman filter gain group:

$$\mbox{(2) predict covariance: }P_{k|k-1} = AP_{k-1|k-1}A^T + Q$$

$$\mbox{(4) Innovation Covariance: }S_k = CP_{k|k-1}C^T+R$$

$$\mbox{(5) Kalman gain: }K_k=P_{k|k-1}A^TS_k^{-1}$$

$$\mbox{(7) update covariance: }P_{k|k}=(I-K_kA)P_{k|k-1}$$

Estimation error:

$$\mbox{(3) Innovation: }\tilde y_k = z_k  - C\hat x_{k|k-1} - Du_k$$

Feedback control:

$$\mbox{(6) update state: }\hat x_{k|k}=\hat x_{k|k-1} + K_k\tilde y_k$$

The re-organized equations show that the Kalman filter is a feedback control system,

where the plant is the process model itself, (下圖中的 A z^-1 loop, ignore uk)

the controller is a time varying gain, (下圖中的 Lt, in general Lt is a matrix.  如果用 constant velocity model, 基本上就是 PI controller)

whilst the measurement of the actual process is a reference signal to the feedback system.

From this point of view, if the closed-loop system is stable, then eventually the estimation error will be zero.

It is worth to note that the Kalman filter gain only depends on model parameters and the initial covariance (P0). It is independent from realtime measurement. Hence, the Kalman filter is a time-variant linear system.  

In general Kalman filter 是 time variant linear system, 但在 steady-state (穩態) 時可視為 time invariant linear system.

下圖的上半部是 measurement data generator.  下半部虛線方塊是 Kalman filter 的 feedback interpretation (B=D=0, 暫時忽略 uk,  Lt=Kk).   若 B >< 0 or D >< 0, 只要在上下半部都加入 uk 就 OK.

NewImage

 

Kalman Example1 (P controller)

A simple case is A=C=1.  var(w) = Q;  var(v) = R

可以看出 plant 就是一個 integrator, 因此 e(t) 在穩態會收歛到 0.  

Controller 就是最簡單的 P controller. 

這個 case 在前文一和經討論過,Kalman gain (K or L, between 0 and 1) 基本上由大變小最後收歛到 steady state 的固定值。

K = P- / (P- + R)  where P- = 1/2(Q + sqrt(Q^2+4QR))

if R >> Q    K ~ sqrt(Q/R)

 

以上的架構就是 1st order PLL, loop filter 是一個 constant gain.  直覺來說如果 phase noise 增加 (Q 變大)

PLL 的 loop bandwidth 要增加才能 track 到。相反如果 loop bandwidth 太寬,additive noise (R ) 太多也會造成 error.  因此 loop bandwidth 有一最佳值,K 有最佳值。

PLL 的問題是 time response (exponential response) 和 SNR 會互相 trade-off.   1st order 只有一個 parameter.

另外主要的問題是 phase ramp (frequency offset) 會造成無法 track.  請參考前文。 

 

Kalman Example2 (constant velocity model) (PI controller)

Constant velocity model 也稱為 double integrator model (see MIT OCW under actuated robot)

 A = [1, 1; 0, 1]   B = [1, 0]

 Steady state solution may provide insight to the PI controller setting.

見另文討論 Kalman filter steady state solution.

以 PLL 為例,Kalman filter 是 stochastic signal (i.e. noise) driven 的 time varying filter.  但最後會 settle 到 steady state.  不管在 transient state or steady state, Kalman filter 都是最佳的 phase estimator.  我們可以計算 steady state 的 Kalman filter, 比較 PI controller,  可以找到 PI controller (time invariant) 的最佳設定。

Step1:  solve non-linear DARE equation 得到 steady state covariance P (or Ms)

NewImage

No close form, use root locus?

Step2: 計算 steady state Kalman gain

NewImage

Fs 是 2×1 array, 包含 position gain (P controller) and velocity gain (I controller).

 

以 PLL 為例,可分為兩種類型。First kind:  clock recovery type:  R >> Q.

Second type: phase lock loop from crystal oscillator: Q >> R

Type 1:  Q ~ 0, ==>  Kalman gain ~ 0 (narrow band)

Type 2:  R ~ 0  ==> Kalman gain ~ 1 (wide band) 

 

Case 1:   input phase noise ==> observation noise

VCO noise ==> phase noise ==> process noise?

其他  noise ==> charge pump noise,  sigma-delta noise ==>  how to model?  

 

Use the above formula for PLL with different noise model (input noise or VCO noise)

 

 

Kalman filter constant velocity model –> steady state -> compare with PI controller.

Q –> 3db line width –> cross-over point with white noise, R, –> 決定 filter bandwidth –> Kp and Ki ratio

–> compare with Kalman filter model!!! 

 

 

Case 3:  Sigma delta modulator filter

 

 

 

 

 

 

電路一般的 feedback 不同,是在 input 而非 output; because we use P feedback (resistor or divider).  

Advertisements