本發(fā)明涉及導(dǎo)航定位裝置和方法，特別是涉及一種基于mems傳感器和vlc定位融合的雙卡爾曼濾波導(dǎo)航裝置和方法。

背景技術(shù)：

隨著室內(nèi)定位技術(shù)的發(fā)展和應(yīng)用，基于可見光通信的室內(nèi)定位技術(shù)也在迅速興起并得到廣泛關(guān)注。在充足室內(nèi)光源的環(huán)境下，通過光傳感器等設(shè)備檢測得到經(jīng)過復(fù)用協(xié)議調(diào)制的光信號，通過信號解調(diào)技術(shù)可以將不同光源信號數(shù)據(jù)分離，從而結(jié)合環(huán)境參數(shù)可以計算出定位目標相對每個光源的距離或角度信息，最后通過定位算法如三邊定位可以完成目標定位。

然而，由于目標接收設(shè)備的姿態(tài)會隨著目標移動產(chǎn)生抖動，將對vlc定位結(jié)果帶來較大的影響。另一方面，實際場景中光信號容易被遮擋，將會導(dǎo)致定位不連續(xù)。針對前一個問題，目前的方案主要是通過多傳感器組合共同定位。針對后一個問題，主流的解決方案是通過卡爾曼濾波或者粒子濾波來估計目標位置。

但這些方案存在一些問題：1)相比于單傳感器定位，多傳感器組合定位算法復(fù)雜且成本較高；2)目前定位方案中提出的濾波器融合是以探測器姿態(tài)平穩(wěn)為前提，在實際場景中穩(wěn)定性較差。3)在信號被遮擋的情況頻繁發(fā)生的場景中，僅通過vlc數(shù)據(jù)加濾波器的定位系統(tǒng)推測的目標位置仍然與實際位置有較大偏差，vlc定位結(jié)果不連續(xù)、不平滑；另外，在慣導(dǎo)定位技術(shù)中，多采用一個卡爾曼濾波來進行噪聲處理和定位，然而這樣通過單一濾波器同時定位和測姿會造成姿態(tài)和位置相互影響。

技術(shù)實現(xiàn)要素：

發(fā)明目的：為解決現(xiàn)有技術(shù)的不足，提供一種基于mems傳感器和vlc定位融合的雙卡爾曼濾波導(dǎo)航裝置和方法。

技術(shù)方案：基于mems傳感器和vlc定位融合的雙卡爾曼濾波導(dǎo)航裝置，包括mems傳感器、ins模塊、pdr定位模塊、vlc定位模塊、測姿擴展卡爾曼濾波器和定位擴展卡爾曼濾波器；所述mems傳感器包括加速度計、陀螺儀以及磁力計；

測姿擴展卡爾曼濾波器的輸入包括：陀螺儀采集接收器在xyz方向的角速度信息經(jīng)過ins模塊的機械編排算法處理得到的ins姿態(tài)信息、加速度計采集的接收器在xyz方向的加速度信息以及磁力計采集的接收器相對東、南、西、北方向的角度信息；其中，加速度信息和角速度信息按時間軸順序輸入至測姿擴展卡爾曼濾波器；輸出包括傳遞給prd定位模塊和vlc定位模塊的姿態(tài)誤差向量和反饋給陀螺儀用以噪聲補償?shù)耐勇萜钕蛄浚?/p>

定位擴展卡爾曼濾波器的輸入包括pdr定位模塊輸出的pdr位置信息和vlc定位模塊輸出的位置信息，pdr定位模塊的輸入包括采集的接收器在xyz方向的加速度信息和所述測姿擴展卡爾曼濾波器輸出的姿態(tài)信息；定位擴展卡爾曼濾波器輸出接收器的定位信息。

一種基于所述導(dǎo)航裝置的導(dǎo)航方法，包括以下步驟：

(1)估計接收器姿態(tài)信息

(11)建立a-ekf的狀態(tài)向量；

(12)建立a-ekf的系統(tǒng)模型；

(13)建立a-ekf的觀測模型；

(14)s-ekf濾波輸出姿態(tài)信息；

(2)估計接收器位置信息

(21)建立l-ekf的狀態(tài)向量；

(22)建立l-ekf的系統(tǒng)模型；

(23)建立l-ekf的觀測模型；

(24)l-ekf濾波輸出定位信息。

進一步的，所述步驟(11)中a-ekf的狀態(tài)向量定義為：

x＝[ψbg]^t

其中：ψ為姿態(tài)誤差向量；bg為陀螺偏差向量。

進一步的，所述步驟(12)中先對姿態(tài)矩陣進行坐標系變換，其坐標轉(zhuǎn)移方程為：

其中，是從載體坐標系到導(dǎo)航坐標系的方向余弦矩陣，即姿態(tài)矩陣；和分別是角速度矢量和的斜對稱矩陣；而和表示載體坐標系相對于慣性坐標的轉(zhuǎn)動角速度和導(dǎo)航坐標系相對于慣性坐標的轉(zhuǎn)動角速度；

a-ekf的系統(tǒng)模型為具體展開公式如下：

其中，和分別表示地球自傳的角速度和導(dǎo)航坐標系相對于地心地固坐標系的轉(zhuǎn)動角速度；wg是傳感器噪聲；τbg代表慣導(dǎo)噪聲的相關(guān)時間；wbg是驅(qū)動噪聲；符號“×”表示叉乘。

進一步的，所述步驟(13)中a-ekf的觀測模型包括加速度計觀測模型和磁力計觀測模型，觀測模型的表達式為z＝hx+v，其中，

加速度計觀測模型具體為：

其中，是加速度計讀數(shù)矢量,fⁿ＝-gⁿ＝[00-g]^t是導(dǎo)航坐標系中的特定力矢量，f^b是載體坐標系中的特定力矢量，g是局部重力值，ψ是姿態(tài)誤差,i是單位矩陣，n2是噪聲，[ψ×]表示ψ矢量的斜對稱矩陣，[gⁿ×]表示gⁿ矢量的斜對稱矩陣，此時，h＝-[gⁿ×]，

磁力計觀測模型具體為：

其中，是磁力計讀數(shù)矢量，mⁿ是校準的lmf矢量，n3是噪聲，此時，h＝[mⁿ×]，

進一步的，所述步驟(14)包括：

(a)將系統(tǒng)模型根據(jù)卡爾曼濾波的模型形式進行變換成如下形式：

其中，

(b)觀測模型z＝hx+v對應(yīng)以下方程：

加速計觀測方程為：

此時，h＝[-[gⁿ×]03×3]，

磁力計觀測方程為：

此時，h＝[[mⁿ×]03×3]，

其中，兩個觀測方程根據(jù)時間軸順序參與到算法中；

(c)測姿擴展卡爾曼濾波遞推步驟如下

①設(shè)置初值

令協(xié)方差矩陣p的初始值為：

p0＝diag([var(ψ0)var(bg0)])

其中，ψ0和bg0分別表示向量ψ和bg的初始值，p0表示各個向量初始值的協(xié)方差構(gòu)成的對角矩陣；

設(shè)狀態(tài)量x初始值為0向量；

②利用k-1時刻的協(xié)方差矩陣對k時刻的協(xié)方差矩陣進行預(yù)測，預(yù)測公式為：

pk′＝fpk-1f^t+q

其中，pk-1表示k-1時刻的協(xié)方差矩陣，pk′表示k時刻的協(xié)方差矩陣的預(yù)測值，q＝e[(gw)(gw)^t]；

③利用k-1時刻的狀態(tài)量對k時刻的狀態(tài)量進行預(yù)測，預(yù)測公式為：

其中，表示k-1時刻的狀態(tài)量，表示k時刻狀態(tài)量的預(yù)測值；

④計算k時刻的卡爾曼增益kk′，計算公式如下：

kk′＝pk′h^t(hpk′h^t+r)^-1

其中，r＝e[vv^t]；

⑤利用k時刻的卡爾曼增益估計k時刻的狀態(tài)量的最優(yōu)值，令該最優(yōu)值作為k時刻的狀態(tài)量則：

⑥利用k時刻的卡爾曼增益估計k時刻的協(xié)方差矩陣的最優(yōu)值，令該最優(yōu)值作為k時刻的協(xié)方差矩陣pk，則：

pk＝(i-kkh)pk′；

⑦輸出k時刻狀態(tài)量中的姿態(tài)信息給vlc定位模塊和pdr定位模塊，狀態(tài)量中的陀螺偏差信息反饋給陀螺儀，同時，令k＝k+1，返回步驟②繼續(xù)循環(huán)濾波不斷更新狀態(tài)量。

進一步的，所述步驟(21)中l(wèi)-ekf的狀態(tài)向量定義為：

其中，和λ分別為接收器緯度信息和經(jīng)度信息。

進一步的，所述步驟(22)中使用pdr定位模塊處理加速度計讀數(shù)獲得步數(shù)和步長信息，并從a-ekf獲得方位角信息，然后pdr通過上一時刻的位置接收器方位角和接收器步長來計算當(dāng)前時刻新的位置其系統(tǒng)模型為：

其中和是k-1時刻的方位角，sk是k時刻的步長，下標k-1和k代表步數(shù)，rm和rn分別是是地球的主垂直線中的子午線和曲率的曲率半徑，h是高度。

進一步的，所述步驟(23)中l(wèi)-ekf的觀測模型直接使用vlc輸出的位置信息建立，觀測模型如下：

其中，和分別為pdr預(yù)測的緯度和經(jīng)度，和分別為vlc輸出的緯度和經(jīng)度，和nλ是測量噪聲。

進一步的，所述步驟(24)包括：

(a)l-ekf系統(tǒng)模型的簡化形式為：

xk＝f(xk-1,uk,wk)

其中，

(b)l-ekf觀測模型的簡化形式為：

zk＝h(xk,vk)

其中，

(c)定位擴展卡爾曼濾波的遞推步驟如下：

①設(shè)置初值

令協(xié)方差矩陣p的初始值為：

其中，p0為狀態(tài)量和狀態(tài)量λk的方差構(gòu)成的對角矩陣；設(shè)狀態(tài)量x初始為0向量；

②利用k-1時刻的狀態(tài)量對k時刻的狀態(tài)量進行預(yù)測，預(yù)測公式為：

其中，xk-1表示k-1時刻的狀態(tài)量，表示k時刻狀態(tài)量的預(yù)測值；

③利用雅可比矩陣進行更新系統(tǒng)模型，公式如下：

④利用k-1時刻的協(xié)方差矩陣對k時刻的協(xié)方差矩陣進行預(yù)測，預(yù)測公式為：

其中，pk-1表示k-1時刻的協(xié)方差矩陣，pk′表示k時刻的協(xié)方差矩陣的預(yù)測值，qk＝e[(gw)(gw)^t]，gw＝wk；

⑤利用雅可比矩陣進行更新觀測模型，公式如下：

⑥計算k時刻的卡爾曼增益kk

其中，kk表示k時刻的卡爾曼增益，rk＝e[vv^t]，v＝vk；

⑦利用k時刻的卡爾曼增益估計k時刻的狀態(tài)量的最優(yōu)值，令該最優(yōu)值作為k時刻的狀態(tài)量

其中，

⑧利用k時刻的卡爾曼增益估計k時刻的協(xié)方差矩陣的最優(yōu)值，令該最優(yōu)值作為k時刻的協(xié)方差矩陣pk，計算公式為：

pk＝(i-kkhk)pk′。

⑨最終輸出接收器的定位信息，并令k＝k+1，返回步驟②繼續(xù)循環(huán)濾波不斷更新狀態(tài)量。

有益效果：與現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點：1)將測姿擴展卡爾曼濾波器和定位擴展卡爾曼濾波器融合首次在vlc定位領(lǐng)域使用融合測姿準確估計vlc接收器的姿態(tài)信息，并消除姿態(tài)對vlc定位的影響；2)該融合濾波器設(shè)計可彌補vlc定位結(jié)果不連續(xù)、不平滑的缺點；3)該融合濾波器設(shè)計可在vlc信號被遮擋的情況下使用mems傳感器信息提供定位結(jié)果。另外，測姿擴展卡爾曼濾波器和定位擴展卡爾曼濾波器融合的雙濾波器架構(gòu)具有的優(yōu)勢是將姿態(tài)和位置分開估計，避免它們的誤差相互影響。同時，每個濾波器的結(jié)構(gòu)又相對簡單，便與調(diào)試；算法簡單效率高。

附圖說明

圖1是測姿定位雙卡爾曼濾波器導(dǎo)航裝置結(jié)構(gòu)示意圖；

圖2是俯仰角0°的仿真定位結(jié)果cdf曲線；

圖3是俯仰角5°的仿真定位結(jié)果cdf曲線；

圖4是俯仰角8°的仿真定位結(jié)果cdf曲線；

圖5是接收設(shè)備平放、傾斜5°、傾斜8°時的定位誤差分析結(jié)果。

具體實施方式

下面結(jié)合附圖對本發(fā)明的技術(shù)方案進行詳細的描述。

如圖1所示，基于mems傳感器和vlc定位融合的雙卡爾曼濾波器導(dǎo)航裝置，其特征在于：包括mems傳感器、慣性導(dǎo)航系統(tǒng)(inertialnavigationsystem，ins)模塊、行人航位推算(pedestriandeadreckoning，pdr)定位模塊、可見光通信(visiblelightcommunication，vlc)定位模塊、測姿擴展卡爾曼濾波器(attitudeextendedkalmanfilter，a-ekf)和定位擴展卡爾曼濾波器(locationextendedkalmanfilter，l-ekf)；所述mems傳感器包括加速度計、陀螺儀以及磁力計。

陀螺儀測得的接收器在xyz方向的角速度信息傳遞給ins模塊，通過陀螺機械編排算法處理數(shù)據(jù)得到ins姿態(tài)信息。加速度計測得的接收器在xyz方向的加速度信息傳遞給pdr定位模塊進行位置估計。另一方面，加速度計測得的接收器在xyz方向的加速度信息、磁力計測得的接收器相對東南西北方向的角度信息與ins姿態(tài)信息三者作為測姿擴展卡爾曼濾波器的輸入量。測姿擴展卡爾曼濾波器輸出包含姿態(tài)誤差向量和陀螺偏差向量。陀螺偏差向量反饋給陀螺儀進行噪聲補償，而姿態(tài)誤差向量傳遞給vlc定位模塊和pdr定位模塊幫助定位。vlc定位模塊和pdr定位模塊通過各自的定位算法分別輸出接收器位置信息給定位擴展卡爾曼濾波器。定位擴展卡爾曼濾波器會持續(xù)更新狀態(tài)量并輸出接收器的定位信息(位置、速度)。該裝置將測姿擴展卡爾曼濾波器和定位擴展卡爾曼濾波器融合形成融合濾波器。

系統(tǒng)中有兩點需要注意：對于a-ekf，磁力計和加速計的讀數(shù)會按時間軸順序參與到測姿擴展卡爾曼濾波器的觀測方程中，并非同時參與濾波；對于l-ekf，從pdr定位模塊獲得的位置信息作為定位擴展卡爾曼濾波器預(yù)測和更新的狀態(tài)量，而從vlc定位模塊獲得的位置信息作為觀測量參與到定位擴展卡爾曼濾波器中。

測姿擴展卡爾曼濾波器的輸入包括：陀螺儀采集接收器在xyz方向的角速度信息經(jīng)過ins模塊的機械編排算法處理得到的ins姿態(tài)信息、加速度計采集的接收器在xyz方向的加速度信息以及磁力計采集的接收器相對東、南、西、北方向的角度信息；輸出包括傳遞給prd定位模塊和vlc定位模塊的姿態(tài)誤差向量和反饋給陀螺儀用以噪聲補償?shù)耐勇萜钕蛄俊?/p>

定位擴展卡爾曼濾波器的輸入包括pdr定位模塊輸出的pdr位置信息和vlc定位模塊輸出的位置信息，pdr定位模塊的輸入包括采集的接收器在xyz方向的加速度信息和所述測姿擴展卡爾曼濾波器輸出的姿態(tài)信息；定位擴展卡爾曼濾波器輸出接收器的定位信息。

本發(fā)明基于ins慣導(dǎo)機械編排的誤差方程作為融合濾波器的系統(tǒng)方程，觀測方程包括vlc定位信息更新、pdr定位信息更新和磁力計觀測量更新。融合濾波器輸出vlc接收器的姿態(tài)信息給vlc定位模塊，輸出pdr接收器的姿態(tài)信息給pdr定位模塊以校正姿態(tài)的影響。

一種基于所述導(dǎo)航裝置的導(dǎo)航方法，包括以下步驟：

步驟1：估計接收器姿態(tài)信息

a-ekf主要用來估計接收器的姿態(tài)信息(即：橫滾角、俯仰角和方位角)。

(1)建立a-ekf的狀態(tài)向量

a-ekf的狀態(tài)向量定義為：

x＝[ψbg]^t(1)

其中：ψ為姿態(tài)誤差向量；bg為陀螺偏差向量。

(2)建立a-ekf的系統(tǒng)模型

先對姿態(tài)矩陣進行坐標系變換，其坐標轉(zhuǎn)移方程為：

其中，是從載體坐標系(b-frame)到導(dǎo)航坐標系(n-frame)的方向余弦矩陣；和分別是角速度矢量和的斜對稱矩陣；而和表示載體坐標系(b-frame)相對于慣性坐標(i-frame)的轉(zhuǎn)動角速度和導(dǎo)航坐標系(n-frame)相對于慣性坐標(i-frame)的轉(zhuǎn)動角速度。

a-ekf的系統(tǒng)模型為具體展開公式如下：

其中，和表示地球自傳的角速度和導(dǎo)航坐標系(n-frame)相對于地心地固坐標系(ecef坐標系，e-frame)的轉(zhuǎn)動角速度；wg是傳感器噪聲；τbg代表慣導(dǎo)噪聲的相關(guān)時間；wbg是驅(qū)動噪聲；符號“×”表示叉乘。

(3)建立a-ekf的觀測模型

a-ekf的觀測模型直接使用加速度計和磁力計讀數(shù)建立。本發(fā)明通過使用加速度計讀數(shù)來構(gòu)建觀測模型，而不是應(yīng)用加速度計導(dǎo)出的橫滾角和俯仰角。這個選擇對于具有任意接收器姿態(tài)的行人應(yīng)用是重要的，因為它避免了當(dāng)俯仰角達到±90°時的奇點問題。

a-ekf的觀測模型包括加速度計觀測模型和磁力計觀測模型，觀測模型的表達式為z＝hx+v其中：

加速度計觀測模型具體為：

其中，是加速度計讀數(shù)矢量,fⁿ＝-gⁿ＝[00-g]^t是導(dǎo)航坐標系中的特定力矢量，f^b是載體坐標系中的特定力矢量，g是局部重力值，ψ是姿態(tài)誤差,i是單位矩陣，n2是噪聲，[ψ×]表示ψ矢量的斜對稱矩陣，[gⁿ×]表示gⁿ矢量的斜對稱矩陣，此時h＝-[gⁿ×]，

a-ekf使用磁力計讀數(shù)建立觀測模型的主要挑戰(zhàn)在于存在頻繁的磁擾動。典型類型的磁擾動是局部磁場(localmagneticfield,lmf)的方向和強度都改變，但是該改變在有限的空間(或時間段)內(nèi)是穩(wěn)定的。lmf穩(wěn)定的周期可以稱為準靜態(tài)磁場(quasi-staticmagneticfield,qsmf)周期，并且可以通過使用磁力計讀數(shù)的大小來檢測。本發(fā)明所使用的算法將使用磁力計在qsmf期間的觀測值，假設(shè)完全不知道lmf參數(shù)，并通過算法在每個qsmf周期的開始階段校準lmf。第k個qsmf周期期間的lmf向量下列公式計算：

其中，是在第k個qsmf周期的第一個時期的開始處的磁力計讀數(shù)，然后將計算的用作第k個qsmf周期期間的參考。本發(fā)明直接使用磁力計讀數(shù)建立觀測模型，該策略避免了調(diào)平步驟(即：使用加速度計讀數(shù)來計算橫滾角和俯仰角)。因此，下面列出的磁力計測量模型與加速度計測量值無關(guān)。

磁力計觀測模型具體為：

其中，是磁力計讀數(shù)矢量，mⁿ是校準的lmf矢量，n3是噪聲，此時，h＝[mⁿ×]，

(4)a-ekf濾波輸出姿態(tài)信息

為了進一步實現(xiàn)擴展卡爾曼濾波，需要將系統(tǒng)模型(公式(3))和觀測模型(公式(4)和(6))代入到擴展卡爾曼濾波的時間更新方程和測量更新方程。首先先將上述中系統(tǒng)模型和觀測模型根據(jù)卡爾曼濾波的模型形式進行變換，從而與模型中的變量一一對應(yīng)。

(a)將系統(tǒng)模型根據(jù)卡爾曼濾波的模型形式進行變換成如下形式：

其中，

(b)觀測模型z＝hx+v對應(yīng)以下方程：

加速計觀測方程為：

此時，h＝[-[gⁿ×]03×3]，

磁力計觀測方程為：

此時，h＝[[mⁿ×]03×3]，

其中，由于加速度計和磁力計從測量到返回讀數(shù)不一定同步，且和二者的讀數(shù)頻率有關(guān)，所以二者作為輸入量在時間上是錯開的。同時a-ekf濾波器在一個時刻只能接收一個觀測方程，即兩個觀測方程根據(jù)時間軸順序參與到算法中。

確定好各個變量以后，就可以按照擴展卡爾曼濾波的時間更新方程和測量更新方程遞推進行濾波。

(c)測姿擴展卡爾曼濾波遞推步驟如下

①設(shè)置初值

令協(xié)方差矩陣p的初始值為：

p0＝diag([var(ψ0)var(bg0)])(10)

其中，ψ0和bg0分別表示向量ψ和bg的初始值，p0表示各個向量初始值的協(xié)方差構(gòu)成的對角矩陣；

設(shè)狀態(tài)量x初始值為0向量；

②利用k-1時刻的協(xié)方差矩陣對k時刻的協(xié)方差矩陣進行預(yù)測，預(yù)測公式為：

pk′＝fpk-1f^t+q(11)

其中，pk-1表示k-1時刻的協(xié)方差矩陣，pk′表示k時刻的協(xié)方差矩陣的預(yù)測值，q＝e[(gw)(gw)^t]；

③利用k-1時刻的狀態(tài)量對k時刻的狀態(tài)量進行預(yù)測，預(yù)測公式為：

其中，表示k-1時刻的狀態(tài)量，表示k時刻狀態(tài)量的預(yù)測值；

④計算k時刻的卡爾曼增益kk′，計算公式如下：

kk′＝pk′h^t(hpk′h^t+r)^-1(13)

其中，r＝e[vv^t]；

⑤利用k時刻的卡爾曼增益估計k時刻的狀態(tài)量的最優(yōu)值，令該最優(yōu)值作為k時刻的狀態(tài)量則：

⑥利用k時刻的卡爾曼增益估計k時刻的協(xié)方差矩陣的最優(yōu)值，令該最優(yōu)值作為k時刻的協(xié)方差矩陣pk，則：

pk＝(i-kkh)pk′(15)

⑦輸出k時刻狀態(tài)量中的姿態(tài)信息給vlc定位模塊和pdr定位模塊，狀態(tài)量中的陀螺偏差信息反饋給陀螺儀，同時，令k＝k+1，返回步驟②繼續(xù)循環(huán)濾波不斷更新狀態(tài)量。

其中，步驟②、③中的方程為時間更新方程，④、⑤、⑥中為測量更新方程。

步驟2：估計接收器位置信息

l-ekf主要用來估計接收器的二維位置信息(即：緯度和經(jīng)度)。圖1顯示l-ekf的輸入為1)pdr得出的位置信息和2)vlc得出的位置信息；輸出為接收器的位置信息。

(1)建立l-ekf的狀態(tài)向量

l-ekf的狀態(tài)向量定義為：

其中，和λ分別為接收器緯度信息和經(jīng)度信息。

(2)建立l-ekf的系統(tǒng)模型

使用行人航位推算pdr定位模塊處理加速度計讀數(shù)獲得步數(shù)和步長信息，并從a-ekf獲得方位角信息，然后pdr通過上一時刻的位置接收器方位角和接收器步長來計算當(dāng)前時刻的位置其系統(tǒng)模型為：

其中表示k-1時刻的方位角，sk表示k時刻的步長，rm和rn分別表示地球的主垂直線中的子午線和曲率的曲率半徑，h是高度，和wλ為位置噪聲。

(3)建立l-ekf的觀測模型

l-ekf的觀測模型直接使用vlc輸出的位置信息建立，觀測模型如下：

其中，和分別為pdr預(yù)測的緯度和經(jīng)度，和分別為vlc輸出的緯度和經(jīng)度，和nλ是測量噪聲。

(4)l-ekf濾波輸出定位信息

為了進一步實現(xiàn)擴展卡爾曼濾波，需要將l-ekf系統(tǒng)模型公式和觀測模型公式代入到定位擴展卡爾曼濾波的時間更新方程和測量更新方程。

l-ekf系統(tǒng)模型公式是對離散過以后的形式，所以濾波過程與a-ekf略有差異。濾波過程包括確定系統(tǒng)模型和觀測模型并進行遞推濾波步驟：

(41)l-ekf系統(tǒng)模型的簡化形式為：

xk＝f(xk-1,uk,wk)(19)

其中，

(42)l-ekf觀測模型的簡化形式為：

zk＝h(xk,vk)(20)

其中，

(43)定位擴展卡爾曼濾波的遞推步驟如下：

(a)設(shè)置初值

令協(xié)方差矩陣p的初始值為：

其中，p0為狀態(tài)量和狀態(tài)量λk的方差構(gòu)成的對角矩陣；設(shè)狀態(tài)量x初始為0向量；

(b)利用k-1時刻的狀態(tài)量對k時刻的狀態(tài)量進行預(yù)測，預(yù)測公式為：

其中，xk-1表示k-1時刻的狀態(tài)量，表示k時刻狀態(tài)量的預(yù)測值；

(c)利用雅可比矩陣進行更新系統(tǒng)模型，公式如下：

(d)利用k-1時刻的協(xié)方差矩陣對k時刻的協(xié)方差矩陣進行預(yù)測，預(yù)測公式為：

其中，pk-1表示k-1時刻的協(xié)方差矩陣，pk′表示k時刻的協(xié)方差矩陣的預(yù)測值，qk＝e[(gw)(gw)^t]，gw＝wk；

(e)利用雅可比矩陣進行更新觀測模型，公式如下：

(f)計算k時刻的卡爾曼增益kk

其中，kk表示k時刻的卡爾曼增益，rk＝e[vv^t]，v＝vk；

(g)利用k時刻的卡爾曼增益估計k時刻的狀態(tài)量的最優(yōu)值，令該最優(yōu)值作為k時刻的狀態(tài)量

其中，

(h)利用k時刻的卡爾曼增益估計k時刻的協(xié)方差矩陣的最優(yōu)值，令該最優(yōu)值作為k時刻的協(xié)方差矩陣pk，計算公式為：

pk＝(i-kkhk)pk′(28)

(i)最終輸出接收器的定位信息，并令k＝k+1，返回步驟(b)繼續(xù)循環(huán)濾波不斷更新狀態(tài)量。

其中，從步驟(b)到(d)為時間更新過程，步驟(c)到(h)為測量更新方程.

技術(shù)原理：

在vlc定位模塊中，定位算法一般采用三邊定位算法。其原理是利用室內(nèi)頂端的多盞燈(一般至少有三盞led燈)作為發(fā)射源，通過接收器實時測得的光信號強度代入光信號傳播模型估計出目標點到每盞燈的距離，最后通過聯(lián)立方程估算出定位位置。然而，當(dāng)光信號被遮擋時，探測器無法獲取全部信號，求解出的位置會嚴重超出誤差范圍，這種情況會通過閾值進行排除，所以系統(tǒng)在該情況下不輸出位置信息，導(dǎo)致定位結(jié)果不連續(xù)。另外，考慮到vlc系統(tǒng)往往存在散粒噪聲和熱噪聲等隨機噪聲，距離的估計往往存在一定誤差，因此估得的位置形成的運動軌跡會存在陡峭變化等特征，這往往不符合實際目標的運動特征。由于在目標跟蹤時，先驗知識應(yīng)當(dāng)表示定位軌跡平滑，目標當(dāng)前時刻的狀態(tài)與上一時刻的狀態(tài)相關(guān)，而濾波方法可以將先驗值考慮進來，從而使得定位軌跡更加平滑。

一般的vlc定位往往假設(shè)設(shè)備是平放姿態(tài)，從而簡化了定位算法。然而在實際情況中設(shè)備會產(chǎn)生抖動、傾斜。設(shè)備的隨機傾斜會改變光信號的入射角，從而導(dǎo)致測得的光信號強度發(fā)生變化。如果無法準確估計設(shè)備的姿態(tài)，這部分的光信號強度變化將成為系統(tǒng)誤差的一部分，導(dǎo)致定位誤差變大。為了說明姿態(tài)誤差對于vlc定位精度的影響程度，我們通過對比仿真了不同俯仰角的情況下vlc定位的精度。該仿真實驗對接收器發(fā)生傾斜，但定位算法不校正的情況進行了模擬，對所有接收器仿真了兩種不同的傾斜情況，定位結(jié)果cdf曲線分別如圖3和圖4所示(圖2表示設(shè)備未發(fā)生傾斜的定位結(jié)果)。實驗結(jié)果顯示接收器俯仰角為5°和8°時最大定位誤差分別約為0.13m和0.21m。對比接收器傾斜和平放時的定位結(jié)果(如圖5所示)，可以看出當(dāng)接收器發(fā)生了傾斜，如果不對接收器姿態(tài)進行修正，定位誤差會顯著的增大。該仿真結(jié)果證明準確估計接收器的姿態(tài)對提高可見光定位系統(tǒng)的性能具有重要意義。因此，引入mems姿態(tài)估計模塊，將準確的姿態(tài)信息傳遞給vlc定位模塊進行姿態(tài)校準，能夠顯著的減小姿態(tài)對vlc定位結(jié)果的影響。

由于光信號在被遮擋的情況下vlc定位無法進行，因此引入pdr模塊進行定位。pdr定位可以完全利用mems傳感器信號來計算步長和方向，利用行人上一時刻的位置推測出行人當(dāng)前的位置。通過反復(fù)迭代，即可得到行人的運動軌跡。因此，當(dāng)vlc定位發(fā)生信號遮擋時(無法輸出位置結(jié)果)，融合系統(tǒng)會采用pdr模塊的定位結(jié)果進行彌補。

單濾波器結(jié)構(gòu)姿態(tài)和位置誤差間相互影響：由于濾波器中的系統(tǒng)模型利用了慣導(dǎo)定位的位置預(yù)測公式。而在慣導(dǎo)中，速度誤差矢量的狀態(tài)轉(zhuǎn)移包含姿態(tài)變量，位置誤差矢量的狀態(tài)轉(zhuǎn)移又和速度相關(guān)。因此，當(dāng)采用單濾波器的架構(gòu)時，姿態(tài)和位置信息同時預(yù)測估計，必然會造成姿態(tài)的預(yù)測估計誤差影響位置信息的預(yù)測。所以本發(fā)明通過采用雙濾波器架構(gòu)將姿態(tài)和位置的預(yù)測估計過程解耦，避免兩者之間相互影響。