本文使用 zhihu on vscode 創作並發布寫在開頭從全狀態觀測器到卡爾曼濾波器（一）：
從全狀態觀測器到卡爾曼濾波器（二）：
從全狀態觀測器到卡爾曼濾波器（三）：
從全狀態觀測器到卡爾曼濾波器（四）：
我們現在希望用乙個超聲波測距感測器測量固定在直線軌道上的小車位置
其中為時刻測量值，
為時刻溫度，
為為時刻測量雜訊。實際上，狀態觀測器除了能減小測量雜訊對最終結果的影響，更重要的應用是估計不可測的狀態量，比如這個例子中的速度。其中
尤拉方法是一種解決數值常微分方程的最基本的一類顯型方法，以以下微分方程為例，已知
可通過在某點附近的線性近似求得其近似解，利用
時刻的數值，採用單步尤拉方法可得到
時刻的近似解
我們通過尤拉方法得到狀態觀測器的遞推方程，即將觀測器由連續時間形式的微分方程轉為離散時間形式的差分方程
將代入化簡得
其中上面的離散形式存在乙個嚴重的問題，即測量值滯後於估計值乙個週期，因為我們計算
時刻的估計值時使用的是
時刻的反饋
。為解決反饋的滯後問題，
需要將上面的單個差分方程拆分成兩個：**與修正**以得到先驗估計
測量值參與修正以得到後驗估計
這樣一來，我們就得到了全狀態觀測器完整的離散形式，即先**、後修正。
離散狀態觀測器在數字計算平台中的更新頻率即步長的選取不應過長也不應過短。
步長過短會增大單位時間內計算裝置的計算量，在計算裝置算力允許的情況下一般不會有太惡劣的影響。
但步長過長則會導致模型誤差增大從而嚴重影響**精度。以勻速模型為例，步長越長則週期內系統加速度的值或速度的變化就越可能不符合模型的勻速假設。若測量值更新緩慢，如視覺目標跟蹤中**目標位置設定roi的場合，目標位置的測量更新頻率受制於攝像頭幀率與演算法計算速度。可通過提高觀測器階數，即由勻速模型擴充套件為勻加速模型以提高**精度。
對初學者而言，儘管明白狀態觀測器的數學原理，其矩陣的表達形式也難免會讓初學者感到雲裡霧裡。接下來我們通過將上面例子的狀態觀測器中矩陣與向量乘開，稍加變形得到更直觀的表達形式，以觀察這個觀測器是如何得到位置與速度估計的。
簡單起見，我們假設觀測期間系統無輸入，即
，有將矩陣代入**
得到兩個狀態量的先驗估計
設，將矩陣
代入修正
得到兩個狀態量的後驗估計
我們觀察位置的估計
，對其變形，有
我們發現這是乙個典型的互補濾波，對根據模型計算的**值與通過感測器測得的測量值進行互補濾波
再觀察速度的估計值
，我們發現無法像位置估計表示式那樣直接變形，因為等式右側沒有任何同類項。因此我們將上面得到的先驗估計表示式
代入，有
合併同類項得
合併後我們並未得到如位置估計值
那樣顯然的互補形式。我們回顧離散時間域中計算訊號一階導數的常用方法——後向差分：
我們發現式
中恰好具有後向差分分子的形式，故做如下變形
代入速度估計表示式中，有
得到訊號一階差分形式的同時，也恰好將等式右側配置為了互補形式，即對訊號後向差分得到的一階導做一階低通濾波。
通過將狀態觀測器展開成標量代數表示式，我們發現狀態觀測器並沒有想象中的那麼玄學。有了對狀態觀測器原理的一定直觀理解，我們將上面一維的例子擴充套件到二維，即通過兩個感測器分別測量小車在平面中的
位置
，系統輸入為分別施加在小車沿方向上的力
，設計狀態觀測器以獲取小車在平面中的座標與速度。回顧一維問題中的狀態向量
擴充套件到二維直接新增
即可根據運動學公式，有
則狀態轉移矩陣
為根據牛頓第二定律，有
設系統輸入向量為
則控制矩陣
為考慮**方程的標準形式
將有關向量和矩陣代入，得到離散時間狀態觀測器的**方程
考慮到我們有兩個感測器分別測量小車在平面中的
位置，故設觀測器輸出為
即輸出矩陣
為將感測器測量值加入修正過程即可得到狀態觀測器的完整形式，這裡我們仍不考慮雜訊，根據觀測器輸出的定義
，設測量向量
為考慮修正方程標準形式
將有關向量和矩陣代入，得到離散時間狀態觀測器的修正方程
最後再通過極點配置確定
矩陣的值即可完成狀態觀測器的設計。
互補濾波係數 3 互補濾波器
首先，我們澄清一點很多人都在討論的問題 卡爾曼濾波器是不是真的優於互補濾波器？答案是肯定的，但有一些前提 感測器的雜訊是隨機的，服從正態分佈的，我們大概知道雜訊的協方差矩陣 並且 優於 是在最小均方誤差 least mean squared error 的意義上。參考文獻 1 p.284 中對於互補...
卡爾曼濾波器
協方差 用於表示兩個變數的總體誤差，如果兩個變化趨勢一致則協方差為正值，變化趨勢不一致則為負值。從直觀上來看，協方差表示的是兩個變數總體誤差的期望。由k 1時刻的最優值和系統輸入計算k時刻的系統 值。根據k 1時刻的系統協方差 k時刻系統協方差。根據 k時刻 協方差矩陣的 值計算卡爾曼增益。根據狀態...
Python 卡爾曼濾波器實現
去年我們在設計一款新產品的時候，由於選用定製開發的乙個soc器件，導致我們在用adc讀取經由這個soc晶元放大後的訊號時，出現了極其不穩定的情況。正常情況下adc讀取出來的訊號應當為一條平穩的直線，而現實上讀取出來的訊號確上下波動極其大，遠遠超出了我們理論計算水平。雖然後來通過大量的研究分析，得出時...