TwinCAT 3自定義卡爾曼濾波功能塊使用方法

第6頁TwinCATTwinCAT3自定義卡爾曼濾波功能塊使用方法作者：袁英杰職務(wù)：華東區(qū)技術(shù)支持工程師郵箱：yj.yuan@日期：2023-03-19摘要：本文介紹自定義庫函數(shù)KalmanFilter的使用方法。本文將會(huì)先簡單介紹介紹卡爾曼濾波的基本原理，然后介紹該庫下兩個(gè)功能塊的引腳定義及使用方式，最后介紹一個(gè)該庫的應(yīng)用案例。附件：序號(hào)文件名備注1KalmanFilter.library自定義卡爾曼濾波庫歷史版本：免責(zé)聲明：我們已對本文檔描述的內(nèi)容做測試。但是差錯(cuò)在所難免，無法保證絕對正確并完全滿足您的使用需求。本文檔的內(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

卡爾曼濾波如果用戶希望了解卡爾曼濾波的具體推到請參考教材或者相關(guān)論文（不建議百度），卡爾曼濾波僅針對線性序列，如果希望處理非線性序列可以考慮擴(kuò)展卡爾曼濾波、無痕卡爾曼濾波等，它們的基本原理與卡爾曼濾波相當(dāng)，但需要考慮使用泰勒展開對函數(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)算方法，因此此功能塊僅僅針對一維的信號(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)。使用方法針對確定的模型，在輸入輸出中填入系統(tǒng)的所有矩陣，并將測量信號(hào)輸入至NewData。可以調(diào)整調(diào)整Q和R的值以達(dá)到最優(yōu)的濾波效果。請注意系統(tǒng)的可控性和可觀性，這點(diǎn)在報(bào)錯(cuò)中會(huì)體現(xiàn)，如果出現(xiàn)報(bào)錯(cuò)請將clearError置位后復(fù)位。如果系統(tǒng)矩陣過于隨意，可能會(huì)導(dǎo)致估計(jì)狀態(tài)在迭代過程中溢出，這一點(diǎn)在功能塊中進(jìn)行了規(guī)避，如果出現(xiàn)這種錯(cuò)誤，請重新激活配置。在沒有輸入控制的情況下也請將該引腳賦值，為0即可。應(yīng)用Ⅱ輸入信號(hào)為如下的跳變噪聲濾波結(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@