第6頁TwinCATTwinCAT3自定義卡爾曼濾波功能塊使用方法作者：袁英杰職務(wù)：華東區(qū)技術(shù)支持工程師郵箱：yj.yuan@日期：2023-03-19摘要：本文介紹自定義庫函數(shù)KalmanFilter的使用方法。本文將會(huì)先簡單介紹介紹卡爾曼濾波的基本原理，然后介紹該庫下兩個(gè)功能塊的引腳定義及使用方式，最后介紹一個(gè)該庫的應(yīng)用案例。附件：序號(hào)文件名備注1KalmanFilter.library自定義卡爾曼濾波庫歷史版本：免責(zé)聲明：我們已對(duì)本文檔描述的內(nèi)容做測試。但是差錯(cuò)在所難免，無法保證絕對(duì)正確并完全滿足您的使用需求。本文檔的內(nèi)容可能隨時(shí)更新，如有改動(dòng)，恕不事先通知，也歡迎您提出改進(jìn)建議。參考信息：目錄1. 卡爾曼濾波 32. 一維功能塊定義及使用 42.1. 功能塊圖及變量定義 42.2. 使用事項(xiàng) 43. 應(yīng)用Ⅰ 53.1. 有噪聲的周期跳變信號(hào) 53.2. 卡爾曼濾波程序 53.3. 卡爾曼濾波效果 54. 二維功能塊定義及應(yīng)用 64.1. 功能塊圖及變量定義 64.2. 使用方法 65. 應(yīng)用Ⅱ 76. 常見問題 76.1. 局限性 7

卡爾曼濾波如果用戶希望了解卡爾曼濾波的具體推到請(qǐng)參考教材或者相關(guān)論文（不建議百度），卡爾曼濾波僅針對(duì)線性序列，如果希望處理非線性序列可以考慮擴(kuò)展卡爾曼濾波、無痕卡爾曼濾波等，它們的基本原理與卡爾曼濾波相當(dāng)，但需要考慮使用泰勒展開對(duì)函數(shù)項(xiàng)求導(dǎo)。給出如下的差分形式的狀態(tài)空間表達(dá)式xz其中，A是系統(tǒng)矩陣，B是輸入矩陣，H是測量矩陣wk和vppQ和R分別是過程標(biāo)準(zhǔn)差和測量標(biāo)準(zhǔn)差，又由于正態(tài)分布的特性，兩者都為協(xié)方差為了消除噪聲，引入三個(gè)變量：xk稱為真實(shí)值xk稱為預(yù)測值，也稱為先驗(yàn)狀態(tài)估計(jì)值xk稱為估計(jì)值，也稱為后驗(yàn)狀態(tài)估計(jì)值給出卡爾曼增益K，它表征了狀態(tài)最優(yōu)估計(jì)中的預(yù)測誤差和測量誤差的比重給出K時(shí)刻的先驗(yàn)協(xié)方差矩陣Pk和后驗(yàn)協(xié)方差矩陣結(jié)合上文的定義，省去數(shù)學(xué)推導(dǎo)（此處需要明確貝葉斯公式、馬爾科夫鏈的基本定義，并利用矩陣協(xié)方差的定義、矩陣的跡、矩陣微分公式的協(xié)助推導(dǎo)），得到如下五個(gè)表達(dá)式xPK=xP

一維功能塊定義及使用功能塊圖及變量定義輸出和輸入都為LREAL型，可以根據(jù)需求修改變量類型，但需要同時(shí)修改所有內(nèi)部變量的數(shù)據(jù)類型。系統(tǒng)協(xié)方差和測量協(xié)方差的默認(rèn)初值為2和100，可以作為輸入變量修改。使用事項(xiàng)由于TwinCAT缺少有效的矩陣運(yùn)算方法，因此此功能塊僅僅針對(duì)一維的信號(hào)，轉(zhuǎn)移矩陣都為1，矩陣的求逆變?yōu)槌āＴ谧兞柯暶鲄^(qū)定義KalmanFilter類型的變量kalmanfilter，函數(shù)實(shí)例化如下

應(yīng)用Ⅰ有噪聲的周期跳變信號(hào)卡爾曼濾波程序卡爾曼濾波效果綠色曲線是跳變的有噪聲的信號(hào)，藍(lán)色信號(hào)是動(dòng)態(tài)的低通濾波，黃色曲線是卡爾曼濾波的結(jié)果。本例程沒有動(dòng)態(tài)使用卡爾曼濾波，所以其動(dòng)態(tài)性能不如動(dòng)態(tài)濾波器，但穩(wěn)態(tài)響應(yīng)較好，曲線波動(dòng)更小。二維功能塊定義及應(yīng)用功能塊圖及變量定義上側(cè)的四個(gè)信號(hào)位輸入變量，下側(cè)的四個(gè)信號(hào)為輸入輸出變量，不建議修改變量的維數(shù)，這會(huì)導(dǎo)致程序中巨大的改動(dòng)。使用方法針對(duì)確定的模型，在輸入輸出中填入系統(tǒng)的所有矩陣，并將測量信號(hào)輸入至NewData。可以調(diào)整調(diào)整Q和R的值以達(dá)到最優(yōu)的濾波效果。請(qǐng)注意系統(tǒng)的可控性和可觀性，這點(diǎn)在報(bào)錯(cuò)中會(huì)體現(xiàn)，如果出現(xiàn)報(bào)錯(cuò)請(qǐng)將clearError置位后復(fù)位。如果系統(tǒng)矩陣過于隨意，可能會(huì)導(dǎo)致估計(jì)狀態(tài)在迭代過程中溢出，這一點(diǎn)在功能塊中進(jìn)行了規(guī)避，如果出現(xiàn)這種錯(cuò)誤，請(qǐng)重新激活配置。在沒有輸入控制的情況下也請(qǐng)將該引腳賦值，為0即可。應(yīng)用Ⅱ輸入信號(hào)為如下的跳變?cè)肼暈V波結(jié)果如下，觀測矩陣為0.1和1，因此狀態(tài)2（橙色曲線）接近輸出信號(hào)，狀態(tài)1為（藍(lán)色曲線）任意狀態(tài)。常見問題局限性該庫下的兩個(gè)功能塊一個(gè)只能處理一維狀態(tài)無控制信號(hào)，另一個(gè)只能處理二維SISO狀態(tài)信號(hào)，代碼源碼并未編譯，可修改表達(dá)式、或使用外部軟件進(jìn)行矩陣的乘法、轉(zhuǎn)置、求逆等計(jì)算從而處理二維狀態(tài)以上的信號(hào)或者M(jìn)IMO系統(tǒng)。

上海（中國區(qū)總部）中國上海市靜安區(qū)汶水路299弄9號(hào)（市北智匯園）電話 北京分公司北京市西城區(qū)新街口北大街3號(hào)新街高和大廈407室電話 郵箱:beijing@ 廣州分公司廣州市天河區(qū)珠江新城珠江東路32號(hào)利通廣場1303室電話1/2 郵箱:guangzhou@ 成都分公司成都市錦江區(qū)東御街18號(hào)百揚(yáng)大廈2305室電話郵箱:chengdu@