没有合适的资源?快使用搜索试试~ 我知道了~
首页PX4 的 ECL EKF 公式推导及代码解析.pdf
PX4 的 ECL EKF 公式推导及代码解析.pdf
5星 · 超过95%的资源 需积分: 45 79 下载量 130 浏览量
更新于2023-03-16
评论 4
收藏 138KB PDF 举报
PX4 采用 ECL(Estimation and Control Library,估计与控制库)通过 EKF 来进行多传感器信息融合,对此部分进行详细分析。
资源详情
资源评论
资源推荐
PX4 的 ECL EKF 公式推导及代码解析
CUHK 赵祯俊
August 31, 2019
Contents
1 引言 2
2 状态向量 2
3 可用的传感器 2
4 IMU 传播 2
4.1 状态向量的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
4.1.1 四元数状态的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
4.1.2 速度状态的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
4.1.3 位置状态的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
4.1.4 其他状态的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
4.2 协方差的传播 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
5 更新 5
5.1 GPS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
5.2 气压计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
5.3 磁力计 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
5.3.1 三轴磁场的更新 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
5.3.2 真航向的更新 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
5.4 光流 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
5.5 ZED 相机 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
6 附录 9
6.1 PX4 中四元数的传播方程 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
6.2 PX4 中速度的传播方程 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
6.3 PX4 中位置的传播方程 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
6.4 协方差传播中的 F 和 G 计算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
6.4.1 F 的计算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
6.4.2 G 的计算 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
6.5 更新过程的 Jacobian 推导 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
6.5.1 磁力计更新的观测矩阵 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
6.5.2 光流更新的观测矩阵 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
6.5.3 ZED 相机更新的观测矩阵 Jacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
1
1 引言
PX4 采用 ECL(Estimation and Control Library,估计与控制库)通过 EKF 来进行多传感器信息融合,ECL
的官网为 https : //dev.px4.io/zh/tutorials/tuning_the _ecl_e k f .html。
2 状态向量
PX4 的状态向量如下所示:
x
24×1
=
q v p ∆θ
b
∆v
b
m
NED
m
b
v
wind
T
(1)
其中,q 表示 NED 系(北东地 North、East、Down 的世界坐标系)到 Body 系(IMU 的机体坐标系)的旋转
向量的四元数,v 表示在 NED 系的机体速度,p 表示在 NED 系的机体位置,m
NED
为 earth 在 NED 系下的磁场
向量,简称地磁,v
wind
为在北东 (NE) 方向上的风速。
另外,∆θ
b
为角度增量的偏移误差,单位为 rad,注意并非角速度 ω 的偏移误差,两者的关系为:
∆θ
b
= ω
b
∆ t (2)
∆ v
b
为速度增量的偏移误差,单位为 m/s,注意并非加速度计 a 的偏移误差,两者的关系为:
∆ v
b
= a
b
∆t (3)
m
b
为磁力计的偏移误差, 称为罗差,即飞机上磁力计和磁北极的一个差角,磁力计的读数修正了罗差之后所得
到的才是磁航向。
值得说明的是,q = q
B←N
是表示旋转向量的四元数,即表示将同一个系中的一个向量或者点,旋转到另一个
地方。若我们希望得到从 B 系到 N 系的旋转矩阵,则可使用下面公式,详细可参考附录??。
R
N←B
(
vec
q
)
=
q
2
0
+ q
2
1
− q
2
2
− q
2
3
2q
1
q
2
− 2q
0
q
3
2q
0
q
2
+ 2q
1
q
3
2q
0
q
3
+ 2q
1
q
2
q
2
0
− q
2
1
+ q
2
2
− q
2
3
2q
2
q
3
− 2q
0
q
1
2q
1
q
3
− 2q
0
q
2
2q
0
q
1
+ 2q
2
q
3
q
2
0
− q
2
1
− q
2
2
+ q
2
3
(4)
3 可用的传感器
1. IMU:包含 Body 系下的陀螺仪角度增量和加速度计速度增量;
2. GPS:包含 NED 系下的速度和位置;
3. 气压计:提供 NED 系下的高度;
4. 磁力计:提供 NED 系下的偏航角。
4 IMU 传播
4.1 状态向量的传播
下面来分析每来一个 IMU 数据 ∆θ
m
和 ∆v
m
,状态向量是如何从第 k 时刻传播到第 k+1 时刻的。对于三个
bias、earth 的磁场向量和风速是不随 IMU 变化的,因此我们只需讨论旋转、速度和位置。
2
4.1.1 四元数状态的传播
下面我们详细讨论在 PX4 中旋转四元数随着陀螺仪数据的传播过程。飞行器在旋转过程中存在的陀螺仪圆锥
运动和地球自转运动,将会对陀螺仪的测量数据产生误差,可以采用梯度近似积分法去除这些误差,在 PX4 中忽
略了陀螺仪圆锥运动,考虑了地球自转运动的影响,所以真实角增量计算公式如下所示:
∆θ = ∆θ
m
− ∆θ
b
− ∆θ
earth
− ∆θ
n
(5)
其中,∆θ 为真实的角增量,∆θ
m
为陀螺仪测量得到的角增量,∆θ
b
为角增量的偏移误差,∆θ
n
为角增量的噪声,即
∆θ
n
= n
ω
· ∆t,n
ω
为陀螺仪的角速度噪声,∆t 为 IMU 测量数据的帧间间隔。
将修正之后的角度增量转化为四元数增量 ∆q
k
,再使用下列公式,即可得到从 k 时刻到 k+1 时刻旋转向量的
四元数传播,详细推导可参考附录 6.1。
q
k+1
= q
k
⊗ ∆q
k
(6)
4.1.2 速度状态的传播
速度增量解算时含有旋转效应和划桨效应误差,需要对旋转效应和划桨效应进行补偿,一般采用的方法是梯度
近似积分法。在低精度的小型无人机系统中也可以忽略这些影响。在 PX4 中忽略以上误差得到真实速度增量计算
公式如下所示:
∆ v = ∆v
m
− ∆v
b
− ∆v
n
(7)
其中,∆v 为真实的速度增量,∆v
m
− ∆v
n
为加速度计测量得到的速度增量,∆v
b
为速度增量的偏移误差,∆v
n
为
速度增量的噪声,∆t 为 IMU 测量数据的帧间间隔。
那么,可以得到从 k 时刻到 k+1 时刻的速度传播,详细推导可参考附录 6.2。
v
k+1
= v
k
+ R
N←B
· ∆v + g
N
· ∆t (8)
其中,R
N←B
为机体坐标系到 NED 坐标系的旋转矩阵,g
N
= [0, 0, g]
T
为 NED 坐标系下的重力加速度。
4.1.3 位置状态的传播
位置增量计算的过程中同样存在旋转效应和划桨效应误差,也需要通过梯度近似积分法进行处理。在 PX4 中
同样忽略以上误差,得到从 k 时刻到 k+1 时刻的位置传播,详细推导可参考附录 6.3。
p
k+1
= p
k
+
1
2
· (v
k
+ v
k+1
) · ∆t (9)
4.1.4 其他状态的传播
如前所述,三个 bias、earth 的磁场向量和风速是不随 IMU 变化的。
∆θ
k+1
b
= ∆θ
k
b
(10)
∆v
k+1
b
= ∆v
k
b
(11)
m
k+1
N
= m
k
N
(12)
m
k+1
B
= m
k
B
(13)
v
k+1
wind
= v
k
wind
(14)
3
4.2 协方差的传播
下面我们来讨论状态向量的不确定度传播,即协方差传播,首先我们需要写出状态向量的转移矩阵,如下形式,
为了简洁,我们省略了部分上下标:
x
k+1
= F · x
k
+ G · n
k
(15)
q
v
p
∆θ
b
∆v
b
m
NED
m
B
v
wind
k+1
= F ·
q
v
p
∆θ
b
∆v
b
m
NED
m
B
v
wind
k
+ G ·
∆ θ
n
∆ v
n
k
(16)
根据前面推导的第 k+1 时刻的状态向量公式 (6)、(8)、(9) 和 (10)-(14),分别对第 k 时刻的状态 x
k
求 Jacobian
可得 F 和 G,详细推导可参考附录 6.4:
F
24×24
=
F
q
k+ 1
q
k
0
4×3
0
4×3
F
q
k+ 1
∆θ
k
b
0
4×3
0
4×3
0
4×3
0
4×2
F
v
k+ 1
q
k
I
3×3
0
3×3
0
3×3
F
v
k+ 1
∆v
k
b
0
3×3
0
3×3
0
3×2
0
3×4
F
p
k+ 1
v
k
I
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×2
0
3×4
0
3×3
0
3×3
I
3×3
0
3×3
0
3×3
0
3×3
0
3×2
0
3×4
0
3×3
0
3×3
0
3×3
I
3×3
0
3×3
0
3×3
0
3×2
0
3×4
0
3×3
0
3×3
0
3×3
0
3×3
I
3×3
0
3×3
0
3×2
0
3×4
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
I
3×3
0
3×2
0
2×4
0
2×3
0
2×3
0
2×3
0
2×3
0
2×3
0
2×3
I
2×2
(17)
G
24×6
=
G
q
k+ 1
∆θ
k
n
0
4×3
0
3×3
G
v
k+ 1
∆v
k
n
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
3×3
0
2×3
0
2×3
(18)
根据公式 (15) 和 (18),可以得到状态误差矩阵:
Q = G ·
∆ θ
2
nx
0 0 0 0 0
0 ∆θ
2
ny
0 0 0 0
0 0 ∆θ
2
nz
0 0 0
0 0 0 ∆v
2
nx
0 0
0 0 0 0 ∆v
2
ny
0
0 0 0 0 0 ∆v
2
nz
· G
T
(19)
4
剩余17页未读,继续阅读
无用闲士
- 粉丝: 137
- 资源: 2
上传资源 快速赚钱
- 我的内容管理 收起
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
会员权益专享
最新资源
- ExcelVBA中的Range和Cells用法说明.pdf
- 基于单片机的电梯控制模型设计.doc
- 主成分分析和因子分析.pptx
- 共享笔记服务系统论文.doc
- 基于数据治理体系的数据中台实践分享.pptx
- 变压器的铭牌和额定值.pptx
- 计算机网络课程设计报告--用winsock设计Ping应用程序.doc
- 高电压技术课件:第03章 液体和固体介质的电气特性.pdf
- Oracle商务智能精华介绍.pptx
- 基于单片机的输液滴速控制系统设计文档.doc
- dw考试题 5套.pdf
- 学生档案管理系统详细设计说明书.doc
- 操作系统PPT课件.pptx
- 智慧路边停车管理系统方案.pptx
- 【企业内控系列】企业内部控制之人力资源管理控制(17页).doc
- 温度传感器分类与特点.pptx
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论5