基于擴(kuò)展卡爾曼濾波的tdoa與aoa定位方法

1、1.定位的數(shù)學(xué)模型參考節(jié)點(diǎn)數(shù)目為M（），對(duì)于定位系統(tǒng)可以得到M1個(gè)TDOA數(shù)據(jù)和M個(gè)3M?AOA數(shù)據(jù)，則可以有如下非線性方程：??11123...arctan12...iiiiiiidddviMyyviMxx??????????????????????；；是觀測(cè)噪聲。????22iiidxxyy????????22111dxxyy????1iivv?矩陣形式如下：??YhXV????????12213121213131111111222

2、211211...arctanarctan...arctanMMMMMMMMMddddvdvdddyyvdxxxXYVhXvyyyvxxvyyxx???????????????????????????????????????????????????????????????????????????????????????????????????????211M????????????????????????????????????噪聲向量

3、的協(xié)方差矩陣應(yīng)為方陣，其中是TDOA????2121MM???00RRR?????????R?觀測(cè)的方差陣，是AOA觀測(cè)的方差陣。R?2.非視距識(shí)別和TDOAAOA混合定位結(jié)構(gòu)流程圖TDOAAOA混合定位方法的主要思想是使用多種定位方案，通過多樣性復(fù)和來取得更高的定位精度。是在時(shí)刻的狀態(tài)向量，表示的徑向速度。TkkkXrr?????ktkrkr是輸入噪聲向量，其協(xié)方差矩陣為1kW?2uQI??是狀態(tài)轉(zhuǎn)移矩陣，；?101T????????

4、是噪聲輸入矩陣，；T為采樣間隔。?0T????????測(cè)量方程為：kkkYHXV??其中：是觀測(cè)數(shù)據(jù)向量；；kY??10H?為觀測(cè)噪聲序列，其協(xié)方差矩陣為；kV2xRI??卡爾曼濾波函數(shù)有兩個(gè)遞歸步驟組成：一、時(shí)間更新：（1）AA111kkkkkXX?????（2）1111111kkkkTTkkkkkkkkPPQ?????????????（3）??111kkkkTTkkkkkKPHHPHR?????二、測(cè)量更新：（4）AAA??11kk

5、kkkkkkXXKYHX?????（5）??1kkkkkPIKHP???其中，是卡爾曼濾波增益；kK是的協(xié)方差矩陣；kPAkX由于無法事先確定待測(cè)目標(biāo)與參與定位的M個(gè)參考節(jié)點(diǎn)之間的環(huán)境是LOS還是NLOS，可以利用歷史距離測(cè)量值和無偏卡爾曼濾波處理結(jié)果周期性的檢測(cè)待測(cè)節(jié)點(diǎn)與參考節(jié)點(diǎn)之間的LOSNLOS傳播環(huán)境，其中檢測(cè)周期和誤差計(jì)算樣本值有實(shí)驗(yàn)確定。testN對(duì)于每一個(gè)參考節(jié)點(diǎn)，通過收集一組個(gè)TOA原始數(shù)據(jù)周期性的完成LOSNLOSte