[發(fā)明專利]一種應(yīng)用于四旋翼機(jī)器人平臺(tái)的非線性濾波算法

在審

• 申請(qǐng)?zhí)枺?/td>: 201710319979.2
• 申請(qǐng)日: 2017-05-09
• 公開（公告）號(hào): CN107272716A
• 公開（公告）日: 2017-10-20
• 發(fā)明（設(shè)計(jì)）人: 陳俊;汪家民
• 申請(qǐng)（專利權(quán)）人: 安徽機(jī)電職業(yè)技術(shù)學(xué)院
• 主分類號(hào): G05D1/08
• 分類號(hào): G05D1/08
• 代理公司: 北京風(fēng)雅頌專利代理有限公司11403
• 代理人: 楊紅梅

1.一種應(yīng)用于四旋翼機(jī)器人平臺(tái)的非線性濾波算法，其特征在于,包括如下步驟：

步驟一：利用姿態(tài)敏感器的多次測(cè)量輸出多個(gè)測(cè)量結(jié)果，作為服從初始概率密度p(x₀)的N個(gè)粒子，記為x₀(i)，i＝1,2,…,N；

步驟二：系統(tǒng)狀態(tài)轉(zhuǎn)移概率密度中抽取更新粒子，x_k(i)～p(x_k(i)|x_k-1(i))；

步驟三：更新并歸一化粒子權(quán)重ω_k(i)，計(jì)算狀態(tài)估計(jì)值x_k和方差P_k；

步驟四：從步驟三中的ω_k(i)和x_k(i)中進(jìn)行重采樣得到

步驟五：由更新狀態(tài)估計(jì)值和方差P_k。

2.根據(jù)權(quán)利要求1所述的應(yīng)用于四旋翼機(jī)器人平臺(tái)的非線性濾波算法，其特征在于,所述步驟三中，狀態(tài)估計(jì)值x_k和方差P_k的具體求解方式為：

3.根據(jù)權(quán)利要求1所述的應(yīng)用于四旋翼機(jī)器人平臺(tái)的非線性濾波算法，其特征在于,所述步驟四中，具體求解方式為：其中，K_h(·)為核密度函數(shù)，h為核帶寬，為正數(shù)，n是維數(shù)，由Epanechnikov核函數(shù)抽取樣本e_k(i)，計(jì)算

4.根據(jù)權(quán)利要求1所述的應(yīng)用于四旋翼機(jī)器人平臺(tái)的非線性濾波算法，其特征在于,所述步驟五中，狀態(tài)估計(jì)值和方差P_k為：