表示腳尖離地（62% )，E點表示 腳跟觸地（100% )，腳跟觸地到腳掌觸地為接觸階段，腳掌觸地到腳跟離地為站立階段，腳 跟離地到腳尖離地為離地階段，腳尖離地到腳跟觸地為擺布階段。因此，可以從圖3中可以 看出，站位階段占62%，擺布階段占38%。
[0115] 根據(jù)預(yù)存的陀螺儀設(shè)置閾值判斷所述檢測對象是否處于靜止?fàn)顟B(tài)，獲取第一判斷 結(jié)果，即通過三軸陀螺儀獲取到的所述步行者第k時刻（當(dāng)前時刻）的角速度信息與預(yù)存 的陀螺儀設(shè)置閾值進(jìn)行比對，若所述步行者第k時刻（當(dāng)前時刻）的角速度信息小于預(yù)存 的陀螺儀設(shè)置閾值，則表示所述檢測對象處于靜止?fàn)顟B(tài)，若所述步行者第k時刻（當(dāng)前時 亥IJ)的角速度信息大于預(yù)存的陀螺儀設(shè)置閾值，則表示所述檢測對象處于運動狀態(tài)，返回 步驟S3,繼續(xù)判斷；若所述第一判斷結(jié)果為所述檢測對象處于靜止?fàn)顟B(tài)，繼續(xù)執(zhí)行下一步 驟。
[0116] 判斷所述第一判斷結(jié)果是否與所述查詢結(jié)果一致（檢測對象處于靜止?fàn)顟B(tài)對應(yīng) 所述檢測對象處于站位階段，即表示第一判斷結(jié)果與所述查詢結(jié)果一致，或檢測對象處于 運動狀態(tài)即表示所述檢測對象處于擺布階段即表示第一判斷結(jié)果與所述查詢結(jié)果一致）， 若是，則表示不存在潛在誤差點；若否，則表示存在潛在誤差點，執(zhí)行第五步。
[0117] 通過預(yù)定靜止檢測方式，即樸素貝葉斯估計所述潛在誤差點是否處于零速度區(qū) 間，若是，則表示所述檢測對象在第k時刻（當(dāng)前時刻）處于靜止?fàn)顟B(tài)，調(diào)用所述第一處理 模塊113,若否，則表示所述檢測對象在第k時刻處于運動狀態(tài)，則繼續(xù)調(diào)用所述判斷模塊 112,繼續(xù)判斷。在本實施例中，采用以下樸素貝葉斯估計公式來估計，樸素貝葉斯估計公式 為：
[0118] zv = argmaxP (s | zv) P (a | zv) P (zv)
[0119] 其中，P(s | zv)表示腳步接觸地面的結(jié)束時刻的條件概率，
[0122] 其中，〇標(biāo)識正態(tài)分布的方差，〇 2= CXTgait，h為擺布的起始時刻，t為第k時 刻（當(dāng)前時刻），C為一常數(shù)，Tgait表示整個跨步周期的時間長度。P(a|zv)表示在已知零 速度概率條件下的加速度的條件概率。
[0124] 其中，an表示在地球坐標(biāo)系下的加速度。P(zv)表示基于人體運動學(xué)的概率統(tǒng)計 得到第k時刻（當(dāng)前時刻）為零速度的概率，
[0126] 與所述判斷模塊113連接的第一處理模塊114用于利用預(yù)存位置坐標(biāo)信息推算法 推算所述步行者在當(dāng)前時刻的移動位置坐標(biāo)信息，對所述運動數(shù)據(jù)誤差狀態(tài)進(jìn)行誤差狀態(tài) 偏置估計以修正所述運動數(shù)據(jù)誤差狀態(tài)并獲取校準(zhǔn)后的移動位置坐標(biāo)信息。步驟S12是在 檢測到的零速度階段，利用誤差狀態(tài)卡爾曼濾波器，對推算的運動數(shù)據(jù)誤差狀態(tài)進(jìn)行估計， 然后將估計的誤差去修正捷聯(lián)慣導(dǎo)算法所推算的移動位置坐標(biāo)信息。所述第一處理模塊 114具體包括以下功能：
[0127] 通過定位設(shè)備獲取所述步行者的初始移動位置坐標(biāo)信息。
[0128] 利用捷聯(lián)慣導(dǎo)算法，對所述三軸加速度計和三軸陀螺儀采集到的運動數(shù)據(jù)進(jìn)行計 算。本步驟通過公式（6)計算步行者當(dāng)前時刻，即第k時刻的加速度。
[0130] 其中，<為所述步行者第k時刻（當(dāng)前時刻）在地球坐標(biāo)系下X軸/Y軸/Z軸的 加速度，k表示第k時刻（當(dāng)前時刻），即，n表示地球坐標(biāo)系下的X軸/Y軸/Z軸；,為 第k時刻（當(dāng)前時刻）的方向余弦矩陣，通過該方向余弦矩陣將所述傳感數(shù)據(jù)從傳感器坐 標(biāo)系下轉(zhuǎn)換至地球坐標(biāo)系下，k-1為第k-1時刻（前一時刻）； < 為所述步行者第k時刻在 傳感器坐標(biāo)系下第一方向/第二方向/第三方向的加速度，k表示第k時刻（當(dāng)前時刻），b表示傳感器坐標(biāo)系下第一方向/第二方向/第三方向。同理，計算所述步行者第k時刻的 角速度信息、位置信息、速度信息、及角速度變化信息，同樣是通過，為第k時刻（當(dāng)前 時刻）的方向余弦矩陣將所述步行者第k時刻的角速度信息、位置信息、速度信息、及角速 度變化信息從傳感器坐標(biāo)系下轉(zhuǎn)換至地球坐標(biāo)系下。其中，第k時刻（當(dāng)前時刻）的方向 余弦矩陣^^^,的計算公式為：
[0132] 其中，i為第k-1時刻（前一時刻）的方向余弦矩陣將所述步行者 第k時刻的角速度信息、位置信息、速度信息、及角速度變化信息從傳感器坐標(biāo)系下 轉(zhuǎn)換至地球坐標(biāo)系下；SQk表示為基于角速度的矩陣用以表示小的角度變換對方 向變換的影響，SQk的表達(dá)式為
1，2,3表示傳 感器坐標(biāo)系下第一方向/第二方向/第三方向。方向余弦矩陣6^ ，當(dāng)k= 1時，
其中，roll表示翻滾角，
;pitch是俯仰角，
，yaw是航向角。 '指加速度傳感器測得的x軸的數(shù)據(jù)，"r"指加速度傳感器測得的y軸的數(shù)據(jù)，<_' 指加速度傳感器測得的z軸的數(shù)據(jù)；g為重力加速度。在有磁力計的情況下，yaw為磁 力計在水平面上的度數(shù)，沒有磁力計的情況下初始值設(shè)置為〇。13"為3維單位矩陣，即
;%6為在傳感器坐標(biāo)系下所述步行者第k時刻的角速度信息；At表示采 樣時間間隔，即第k時刻（當(dāng)前時刻）與第k-1時刻（前一時刻）之間相差的時間。
[0133] 對 <進(jìn)行兩次積分以獲取所述步行者第k時刻（當(dāng)前時刻）未校準(zhǔn)的移動位置坐 標(biāo)f目息。
[0134] 在所述零速度階段利用誤差狀態(tài)卡爾曼濾波對運動數(shù)據(jù)誤差狀態(tài)進(jìn)行誤差狀態(tài) 偏置估計。誤差狀態(tài)偏置估計包括姿態(tài)誤差估計、角速度誤差估計、位置誤差估計、速度誤 差估計、及加速度誤差估計。
[0135] 所述步行者第k時刻的運動數(shù)據(jù)誤差狀態(tài)可以標(biāo)記為SXk，該位置信息計算誤差 Sxk采用如下誤差向量表示：
[0137] 其中，表示所述步行者第k時刻的姿態(tài)誤差，<5^表示所述步行者第k時刻的 角速度誤差，Srk表示所述步行者第k時刻的位置誤差，SVk表示所述步行者第k時刻的 速度誤差，&〖表示所述步行者第k時刻的加速度誤差，這5個誤差向量都是對應(yīng)3個正交 軸。
[0138] 所述步行者第k時刻的運動數(shù)據(jù)誤差狀態(tài)SXk的計算公式為
[0139] 8 xk= F k 8 Xh+Wh
[0140] 其中，F(xiàn)k表示狀態(tài)轉(zhuǎn)移矩陣，Sxh表示所述步行者第k_l時刻的位置信息計算誤 差，Wh表示所述步行者第k時刻的位置信息計算誤差的噪聲。在本實施例中，由于需要采 用卡爾曼濾波法對誤差進(jìn)行估計，則卡爾曼濾波的量測模型為
[0141] Zk=H8Xk|k+nk
[0142] 當(dāng)腳步處于靜止?fàn)顟B(tài)的時候，加速度和角速度應(yīng)該為0,但是實際采集的數(shù)據(jù) 存在噪聲，這個噪聲經(jīng)過積分后得到速度誤差，另外一個就是實際測量到的角速度誤 差，這兩個值作為誤差狀態(tài)的測量值Zk，S卩
，H為量測誤差，
1I3X3為單位矩陣。
[0143] 校準(zhǔn)后的所述步行者第k時刻的運動數(shù)據(jù)誤差狀態(tài)
[0144] 8 xk= 8 xH+Kk? [Zk_H 8 xk_J
[0145] 其中，Kk為卡爾曼增益，卡爾曼增益Kk=PuHTOlPKHT+Rkr1中Pk為所述步行者第k 時刻為誤差狀態(tài)協(xié)方差矩陣，Pk= (I 所述步行者第k-1 時刻為誤差狀態(tài)協(xié)方差矩陣，Rk為測量噪聲協(xié)方差矩陣。在本步驟中，采用以下公式分別估 計姿態(tài)誤差，角速度誤差、位置誤差、速度誤差、及加速度誤差，具體如下：
[0151] 修正第k時刻（當(dāng)前時刻）運動數(shù)據(jù)誤差狀態(tài)并獲取校準(zhǔn)后的移動位置坐標(biāo)信 息。姿態(tài)誤差估計為采用姿態(tài)誤差，即所述步行者第k時刻的姿態(tài)誤差5奶，通過一姿態(tài)誤 差矩陣SQk對方向余弦矩陣
?進(jìn)行修正，其 中姿態(tài)誤差矩陣況\為
[0152] 角速度誤差估計為采用角速度誤差，即所述步行者第k時刻（當(dāng)前時刻）的角速 度誤差對第k+1時刻（下一時刻）的角速度進(jìn)行補償；其中，下一時刻的角速度=當(dāng)前 時刻的角速度+角速度誤差；
[0153] 位置誤差估計為采用所述步行者第k時刻（當(dāng)前時刻）的位置誤差S rk對當(dāng)前 時刻未校準(zhǔn)的移動位置坐標(biāo)信息進(jìn)行補償；其中，所述當(dāng)前時刻校準(zhǔn)后的移動位置坐標(biāo)信 息=當(dāng)前時刻未校準(zhǔn)的移動位置坐標(biāo)信息一位置誤差；
[0154] 速度誤差估計為采用速度誤差S Vk，所述步行者第k時刻的速度誤差S Vk對當(dāng)前 時刻的速度進(jìn)行補償；其中，所述當(dāng)前時刻的速度=未校準(zhǔn)的所述步行者當(dāng)前時刻的移動 位置坐標(biāo)信息中的速度一速度誤差；
[0155] 加速度誤差估計為采用加速度誤差&(，即所述步行者第k時刻的加速度偏置 對下一時刻的加速度進(jìn)行補償；其中，下一時刻的加速度=當(dāng)前時刻的加速度+加速度誤 差。
[0156] 所述上層濾波單元12用于執(zhí)行上層濾波。所述上層濾波單元12包括：
[0157] 與所述判斷模塊113和第一處理模塊114連接的第二處理模塊121用于查找當(dāng)前 時刻中所述零速度階段的結(jié)束時刻，利用地圖信息，在所述結(jié)束時刻對當(dāng)前時刻校準(zhǔn)后的 移動位置坐標(biāo)信息執(zhí)行預(yù)制修正，并獲取修正后的移動坐標(biāo)位置信息和上一時刻校準(zhǔn)后的 移動位置坐標(biāo)信息的第一差。請參閱圖2,顯示為基于三軸加速度與三軸陀螺儀采集的數(shù) 據(jù)計算的零速度階段與零速度階段結(jié)束的結(jié)束時刻的計算效果示意圖，圖2依次展現(xiàn)了三 軸加速度數(shù)據(jù)，三軸陀螺儀數(shù)據(jù)和基于這2個原始數(shù)據(jù)計算的零速度結(jié)果，其中第三張圖 中，1表示為零速度時刻，0表示非零速度時刻，（零速度即腳步靜止時刻），其中圖中的點 A即零速度階段的結(jié)束時刻，在這個時刻觸發(fā)利用粒子濾波算法，結(jié)合地圖信息來校準(zhǔn)計算 的移動位置坐標(biāo)信息。點A的獲取方法如下：
[0158] 若（（零速度1)&&(零速度k== 0))，則時刻k是零速度階段的結(jié)束時刻。
[0159] 對在所述結(jié)束時刻對當(dāng)前時刻校準(zhǔn)后的移動位置坐標(biāo)信息，結(jié)合地圖信息，進(jìn)行 粒子濾波方法進(jìn)一步進(jìn)行位置校準(zhǔn)具體如下：
[0160] 基于所述目標(biāo)建筑物的輪廓利用粒子濾波算法對位置計算累計誤差進(jìn)行修正。對 于室內(nèi)定位場景，建筑平面信息的利用能夠限定行走軌跡，減少其不確定性。在給出地圖信 息后，相鄰兩步對應(yīng)粒子之間的連線不能穿越墻壁或者其他障礙物。如果幾次嘗試生成新 的有效粒子仍然失敗，那么就將