信號。補償?shù)募铀儆嫓y量408是 從已經(jīng)補償了加速計偏差404(例如來自在前時間步驟中的后驗估計值)和重力412的加速 計206提供的加速計信號。
[0027] 在四元數(shù)Kalman濾波器的預(yù)測步驟(304)期間，四元數(shù)Kalman濾波器216計算起重 機臂的3D角方向414(俯仰角、滾動角和方向角）的預(yù)測，W及角速度傳感器偏差420的預(yù)測。 運些預(yù)測也使用動態(tài)模型422來計算，運在下文更詳細地描述。驅(qū)動四元數(shù)Kalman濾波器的 預(yù)測步驟(304)所使用的測量是補償?shù)慕撬俣葴y量424。
[0028] 速度Kalman濾波器的修正步驟(306)通過使用提供給測量模型407的計算的起重 機臂的速度426來被驅(qū)動。測量模型407,類似動態(tài)模型406,將在下文被進一步描述。提供給 測量模型407的起重機臂的速度426根據(jù)修正的角速度測量424和角速度傳感器202的（在起 重機臂110上的）已知位置428來被計算。如剛剛提到的，使用從角速度傳感器202提供的角 速度信號和角速度傳感器偏差420的后驗估計值來計算修正的角速度測量424。另外注意的 是，在速度Kalman濾波器的修正步驟（302)之后計算的起重機臂的速度402和加速計偏差 404的預(yù)測是起重機臂的速度402和加速計偏差404的后驗估計值。
[0029] 在中間計算步驟(308)期間，執(zhí)行了若干中間計算。運些計算包括使用起重機臂的 速度402的后驗估計值和(使用角速度傳感器偏差420的后驗估計值所計算的）修正的起重 機臂的角速度測量424來計算起重機臂的加速度432。使用計算的起重機臂的加速度432來 對從傾斜計208提供的傾斜計信號應(yīng)用加速度補償434。運個補償從感測到的起重機臂的滾 動角和感測到的起重機臂的俯仰角中去除了起重機臂的加速度分量，由此提供修正的起重 機臂的滾動角測量和修正的起重機臂的俯仰角測量436。將磁力計校準參數(shù)438應(yīng)用到從磁 力計204提供的磁力計信號，由此產(chǎn)生校準的磁力計測量416。校準參數(shù)438可W在初始的對 準過程期間被確定。然后使用校準的磁力計測量416來計算起重機臂110的（相對于其初始 角方向的)方向角422。然后將修正的起重機臂的滾動角和俯仰角測量436和方向角442轉(zhuǎn)換 成四元數(shù)444并且提供給四元數(shù)Kalman濾波器216。應(yīng)該注意的是，轉(zhuǎn)換到四元數(shù)和接下來 使用四元數(shù)只是可W用來將3D角方向參數(shù)化的一種技術(shù)，并且也可W使用許多其他的姿態(tài) 參數(shù)化方法。一些非限制性的例子包括歐拉角、Rodriques參數(shù)、W及方向余弦，運里僅僅列 舉了一些。
[0030] 在中間計算步驟(308)之后，執(zhí)行四元數(shù)Kalman濾波器修正步驟(310)。使用根據(jù) 磁力計信號計算的修正的滾動角和俯仰角434和根據(jù)校準的磁力計測量438計算的（并且被 轉(zhuǎn)換成四元數(shù)444的）方向角442來驅(qū)動該步驟。運些值被提供給測量模型423,運也在下文 中被進一步描述。計算起重機臂的3D角方向414 (例如滾動角、俯仰角和方向角）的后驗估計 值，W及角速度傳感器偏差420。起重機臂的3D角方向414的后驗估計值被用于生成提供給 顯示設(shè)備212的圖像擅染顯示命令。顯示設(shè)備212,如先前所提到的，擅染了起重機臂姿態(tài)和 方向角的圖像。
[0031] 在上文討論中提到，將描述分別在速度Kalman濾波器214和四元數(shù)Kalman濾波器 216中的動態(tài)模型406和422,與將描述分別在速度Kalman濾波器214和四元數(shù)Kalman濾波器 216中的測量模型407和423-樣。為了完整，現(xiàn)在將提供運些描述。首先從速度Kalman濾波 器214開始，其實現(xiàn)的動態(tài)模型在數(shù)學(xué)上表示如下：
其中？sen^f是傳感器的速度向量，品m是起重機臂的測量的角速度向量，是起重機 臂的測量的比力向量，?是局部重力向量，ig是速率巧螺儀偏差向量，是速率巧螺儀測 量噪聲向量，bf是加速計偏差向量，5?是加速計偏差零漂向量，ifl是加速計偏差漂移速 率向量，af是加速計測量噪聲向量，；fl是加速計偏差高斯-馬爾可夫驅(qū)動過程噪聲向量， 曰f是加速計測量噪聲的標準偏差，Ofl是加速計偏差高斯-馬爾可夫驅(qū)動過程噪聲的標準偏 差，Ta是加速計偏差高斯-馬爾可夫過程的相關(guān)時間，Qv,w是由加速計測量噪聲向量和加速 計偏差高斯-馬爾可夫驅(qū)動過程噪聲向量所定義的速度Kalman濾波器過程噪聲向量的功率 譜密度，W及CbN是從起重機臂導(dǎo)航框架到傳感器體框架的方向余弦矩陣。 W及由速度Kalman濾波器214所實現(xiàn)的測量模型在數(shù)學(xué)上表示如下：
其中，除了那些先前定義過的變量，是傳感器相對于起重機臂的旋轉(zhuǎn)中屯、的位 置向量，Og是速率巧螺儀測量噪聲的標準偏差，W及Rv是根據(jù)補償?shù)慕撬俣葴y量和傳感器位 置向量所計算的起重機臂角速度的協(xié)方差矩陣。
[0032] 在四元數(shù)Kalman濾波器216中實現(xiàn)的動態(tài)模型在數(shù)學(xué)上表示如下：
其中，除了那些先前定義過的變量，5^是估計的四元數(shù)的向量誤差分量，i gp是速率 巧螺儀偏差零漂向量，b gl是速率巧螺儀偏差漂移速率向量，；gl是速率巧螺儀偏差高斯- 馬爾可夫驅(qū)動過程噪聲向量，Ogi速率巧螺儀偏差高斯-馬爾可夫驅(qū)動過程噪聲的標準偏差， Tg是速率巧螺儀偏差高斯-馬爾可夫過程的相關(guān)時間，W及Qq,w是由速率巧螺儀測量噪聲向 量和速率巧螺儀偏差高斯-馬爾可夫驅(qū)動過程噪聲向量所定義的四元數(shù)Kalman濾波器過程 噪聲向量的功率譜密度。 W及由四元數(shù)Kalman濾波器216所實現(xiàn)的測量模型在數(shù)學(xué)上表示如下：
其中，除了那些先前定義過的變量，q是估計的四元數(shù)的向量分量，^4是估計的四元 數(shù)的標量分量，習(xí)是根據(jù)從傾斜計信號計算的修正的滾動角和俯仰角434,和從校準的磁力 計測量438計算的相關(guān)方向角442所計算的四元數(shù)的向量分量，Q4是根據(jù)從傾斜計信號計算 的修正的滾動角和俯仰角434,和從校準的磁力計測量438計算的相關(guān)方向角442所計算的 四元數(shù)的標量分量，〇im:,x是滾動角傾斜計測量的標準偏差，〇inc:,y是俯仰角傾斜計測量的標 準偏差，Omag是根據(jù)校準的磁力計測量438所計算的方向角測量的標準偏差，W及Rq是根據(jù) 從傾斜計信號計算的修正的滾動角和俯仰角434,和從校準的磁力計測量438計算的相關(guān)方 向角442所計算的起重機臂姿態(tài)和方向角測量的協(xié)方差矩陣。
[0033] 運里所公開的起重機臂的姿態(tài)和方向參考系統(tǒng)和方法可W被用于確定起重機臂 的姿態(tài)和方向角。如果所公開的系統(tǒng)被安裝在其他起重機中，相同的信息可W從在特定場 地工作的其他起重機臂提供。起重機臂的姿態(tài)和方向參考系統(tǒng)與其他姿態(tài)和方向參考系統(tǒng) 區(qū)別在于它實現(xiàn)了兩階段Kalman濾波器(例如，速度Kalman濾波器214和四元數(shù)Kalman濾波 器216)來估計起重機臂的姿態(tài)和方向角，估計起重機臂的速度W從加速計測量中移除加速 度分量，由此修正傾斜計測量，W及起重機臂的速度的測量基于的是起重機臂的動力學(xué)特 性。
[0034] 本領(lǐng)域技術(shù)人員將理解，與在運里公開的實施例相結(jié)合描述的各種說明的邏輯 塊、模塊、電路和算法步驟可W被實現(xiàn)為電子硬件、計算機軟件或者兩者的組合。在上文按 照功能和/或邏輯塊部件(或模塊）W及各種處理步驟來描述了一些實施例和實現(xiàn)方式。然 而，應(yīng)當理解的是，運些塊部件(或模塊)可W由配置成執(zhí)行特定功能的任何數(shù)量的硬件、軟 件和/或固件部件來實現(xiàn)。為了清楚地說明硬件和軟件的運種可交換性，各種說明性的部 件、塊、模塊、電路和步驟在上文按照它們的功能性被一般地描述。運種功能性是實現(xiàn)為硬 件還是軟件取決于對整個系統(tǒng)所施加的特定應(yīng)用和設(shè)計約束。本領(lǐng)域技術(shù)人員可W對每一 個特定的應(yīng)用W變化的方式來實現(xiàn)所描述的功能性，但是運種實現(xiàn)決定不應(yīng)當被解釋為引 起了從本發(fā)明的范圍的偏離。例如