早安，苦命工程師的胡言亂語

前言：

最近幾天，被武漢肺炎攻進家裡囉，家裏有人中獎。然後自己也被迫關在家裡照顧他們。在家裏，還是不要浪費寶貴的時間，來寫寫並更新部落格吧。

最近因為操作一些高靈敏度感測器的關係，發現我們常會需要一種非常重要的應用：某些非常高靈敏度的感測器，因為非常的靈敏，所以相對的，通常會有大量的雜訊伴隨在真實訊號裡面。如何在一堆雜訊裡面抽出真實的感測器訊號，是常常會遇到且必須的重要技術。

這種情況，我們通常會分析雜訊的頻率狀況，假如頻率確定的話，適當引進高通濾波器 (High-Pass Filter)，低通濾波器 (Low-Pass Filter)或是帶通濾波器 (Band-Pass Filter)，即可有效的過濾雜訊的干擾。

可是，萬一雜訊的產生頻率不固定，到處跑的話，這時候這些方法就會失效或者是效果不好。

我們可以把最後的法寶，卡爾曼濾波器 (Kalman Filter) 給請出來囉，這個超級威力的，可以適用在各種的情況。卡爾曼濾波器就是非常擅長在一堆有高斯雜訊干擾的情況下，將真實的訊號給抽離出來。

背後的統計和數學理論雖然複雜，但在我們的這個應用中，因為是一維的卡爾曼濾波器，所以純粹實作上並不複雜，只是幾個公式在迭代而已。所以當然，這裡做個心得筆記囉，以後要用時也方便我回來查閱跟參考。

然後，一樣這裡只提供 FORTH 程式碼，用來達到推廣 FORTH 的目的。

卡爾曼濾波器的理論：

這裡是一維的應用，在這個狀況下卡爾曼濾波器的五個公式就變成非常簡單的形式。這裡理論基礎的精神就不贅述了，有興趣可以參考這篇部落格。

簡單介紹所有參數

|x> 系統的狀態：

原來是個向量。這裡因為是一維的，所以是個變數，用來標誌現在系統的狀態。 這個符號 |> 叫做 ket，是量子力學大師 Dirac 發明的，所以物理系學生比較愛用。

P 系統共變數：

系統共變數矩陣。這裡因為是一維的，所以是個變數，用來標誌現在系統的變異狀態。

Q 過程雜訊變異：

系統過程雜訊變異共變數矩陣。這裡因為是一維的，所以是個變數，用來標誌當每次系統狀態轉變時，這個不完美的實際過程中所造成的變異程度。

R 量測雜訊變異：

量測雜訊變異共變數矩陣。這裡因為是一維的，所以是個變數，用來標誌當每次量測時，量測雜訊所引進的變異程度。

理論計算，

(1) 預測

  |x'> = |x>

  P' = P + Q

(2) 更新

  KG = P / (P+R)

  |x'> = |x> + KG*(|z> - |x>)

  P' = (1-KG)*P 

這樣就完工囉，

結果測試

最後我們用之前 101 大樓量測的範例數據來測試一下，

這裡要說明的，我們假設量測時的雜訊很大，所以 R 值很大。

而大樓是固定不動的，只偶而受到強風的擾動，所以 Q 值很小。

有文獻探討當 Noise Reducer 時的 Kalman Filter，它們的 R 和 Q 值要如何設定，此時效果最好呢？

最後他們的結論是 R/Q = 100 左右效果最好。

所以這裡，我們取的參數為: R = 1， Q = 0.01

經過計算，

其結果如下

整理如下，

作圖可以發現，雜訊消失囉，慢慢抽出跟收斂到有用的資訊

FORTH 程式解說，

這裡要說明的，整個 ANSI FORTH 的標準，浮點數的變數用 fvariable 來定義。

取用用 f@ ，存入用 f!

這樣的操作，在一般 FORTH 變數的使用沒有問題。但是對浮點數而言，一般我們使用浮點數大部分在寫科學函數。 f@ 跟 f! 的干擾會讓整個函數的全貌難以辨識跟閱讀。

所以還是採用 value 跟 to 的這種架構比較好些，比較清楚。

但是 ANSI FORTH 對於浮點數並沒有 value 跟 to 的指令。

對於 FORTH， OK 的，沒有我們就自己來定義吧！

仿效 value 跟 to ，我們來定義一個浮點數版本的 fvalue 跟 fto

: fvalue ( F: f --)
     create f, does> f@
;

就很單純， create 的時候將浮點數存入記憶體中。 執行時，直接把所記憶體中的浮點數值 f@ 取出。

: fto ( F: value "var" --)
   ' >body
   state @
   if postpone literal postpone f!
   else f! then
; immediate

' 是個指令，會把 fto 後面的詞的 code field 找出來， >body 轉成 parameter field 參數區的位置(位址)。 fvalue 所定義出來的浮點變數內容就存在這裡。

state @ 用來確認一下，現在是在編譯態還是翻譯態。

如果是在編譯態的話，利用 literal 來將這個位址編進去程式碼中。但是因為 literal 是個立即詞，這樣做的話是編譯不進去 fto 的，所以前面要加上 postpone 來強迫 literal 被編譯進 fto，且執行 fto 時它才真的會被執行。

postpone f! 則是把 f! 這個指令編譯進去字典中。所以最後就是 | (literal) | addr | f! | 這樣的碼被編入字典，亦即執行時會將 addr 放入參數堆疊，然後 f! 將某浮點數存入此 addr 中。

如果是在翻譯態的話，因為 ' >body 已經找好位址了，所以直接 f!

最後，這個 fto 是個用來打造編譯結果的指令(或是翻譯態下立刻執行的指令)，所以加上 immediate 讓它變成編譯器字。這樣編譯的時候它會被立刻執行，而不會被編譯進去程式碼中。

這裡再多定義一個很有用的指令 F# ，用來指名後面的文字是浮點數。

在 FORTH 裡面，因為小數點 . 早就被雙整數辨識拿去使用了。 例如 123.456 會被 FORTH 認為是個 123456 的雙整數，而不是浮點數。

為了辨識浮點數，所以 ANSI FORTH 規定，只能採用科學記號表示法。 例如浮點數字 9，需採用 9E0 這樣的科學記號形式。否則不會被認為是浮點數字。這樣結果就是一堆 3.1415926E0 ， 2.718281828E0， 0.01E0， 1E-2 ... 一堆比較不容易閱讀的形式。

改成 F# 3.1415926    F# 1.0     F# 2.71828   F# 0.01   這樣的形式比較容易閱讀多了。

來加上這樣的指令 F# 吧

: F# ( " -- F: f)
   BL word count >float
   invert abort" Failed to convert to float point..."
   state @
   if postpone fliteral then
; immediate

BL word 以空白字元切出後面的文字，並包成 counted-String。 count 將字串大小取出來，形成 addr n 的形式。

>float 嘗試將 addr n 所指示的字串資料轉換成浮點數，成功的話放上 -1 ，失敗的話放上 0。

invert abort" 浮點數轉換失敗的話就 abort 啦。

成功的話， state @ 看看是不是編譯態，是的話 postpone fliteral 利用 fliteral 將浮點數編譯入程式碼中。

最後 immediate 來讓 F# 是個編譯器指令，主要負責編譯器動作或是翻譯態下立即執行使用。

最後， FORTH 程式碼列表

\
\ Kalman Noise Reducer
\   Frank Lin 2020.9.4
\

: fvalue ( F: f --)
   create f,   does> f@
;

: fto   ( F: value "var" --)
   ' >body
   state @
   if    postpone literal  postpone f!
   else  f!  then
; immediate

: F# ( " -- F: f)
   BL word count >float
   invert abort" Failed to convert to float point..."  
   state @
   if   postpone fliteral then
; immediate

F#    0   fvalue |x>
F#    1   fvalue P
F#    0   fvalue KG
F#    1   fvalue R
F# 0.01   fvalue Q

: |x'> ( F: measure --)  \ |x'> = |x> + KG*(|z> -|x>)
   |x> f-   KG f*   |x> f+
   fto  |x>
;

: P' ( --)               \ P' = P + Q
   P Q f+  fto P
;

: KG' ( --)              \ KG' = P / (P + R)
   P   fdup  R f+   f/  fto KG
;

: P'' ( --)              \ P'' = (1-KG)*P
   F# 1  KG  f-     P  f*
   fto P
;

: predict ( --)
   P'
;

: fusion ( F: measure --)
   KG'
   |x'>
   P''
;

: KF_Reducer ( F: measure -- filted-value)
   predict
   fusion
    
   |x>
;

\
\ Test
\

cr

F#   1.0   fto  P   \ state covariance
F#   1.0   fto  R   \ measurement covariance
F#   0.01  fto  Q   \ process noise covariance
F# 470.8   fto  |x> \ initial state

F# 542.0   KF_Reducer f. cr
F# 404.5   KF_Reducer f. cr
F# 539.5   KF_Reducer f. cr
F# 499.8   KF_Reducer f. cr
F# 513.7   KF_Reducer f. cr
F# 550.0   KF_Reducer f. cr
F# 504.9   KF_Reducer f. cr
F# 450.0   KF_Reducer f. cr
F# 431.0   KF_Reducer f. cr

xxx