表达由机器人位置 (x, y) 引起的对路标预测误差误差

文章插图

对路标的预测对路标坐标的 Jacobian

文章插图

3.8 过程噪声 Q 和 W
过程噪声是与控制信号成比例的高斯噪声 ， 记作 Q 。如果

文章插图

C 与里程计的准确性有关 ， 准确性高对应的系数就大 ， 通常是需要实验调参的 。
3.9 测量噪声： R 和 V
观测噪声是与路标测量 (range,bearing)有关的 2×2 矩阵 ， 
形如

文章插图

其中 r 与 range 有关 ，  c 是常数 ，  如果 range 误差 1cm ，  c 应该取值 0.01 ， 表示高斯噪声方差 。
如果 bearing 误差 1 度 ，  b=1 。
常数与测量设备的准确性有关 。
有了上面几个关键矩阵的铺垫 ， 下面终于 EKF 流程了：
3.10 使用里程计更新系统当前状态
更新的是状态矩阵 X(1.3.2) 前 3 个量

文章插图


文章插图


文章插图

3.11 更新预测模型 Jacobian

文章插图

3.12 更新过程噪声
过程噪声是与控制紧密相关的 。

文章插图

3.13 更新机器人位姿有关的协方差
(协方差矩阵顶部 3 × 3) 块 。

文章插图


文章插图

3.14 用重新观测的路标数据更新当前状态
预测路标
仅从机器人位置信息更新状态是不够准确的 ， 还可以通过对路 标点的重新观测修正机器人位姿 。对于机器人当前位姿 (x, y) 和上一时刻路 标坐标 (λx, λy) 可以计算距离和角度 h：

文章插图


文章插图

还可以直接通过传感器计算路标 。

文章插图

3.15 更新测量误差矩阵 R ，  V

文章插图

其中 r = range, c = 0.01, bd = 1
3.16 计算 kalman 增益

文章插图


文章插图

3.17 新的状态向量

文章插图

3.18 把新的路标加入状态
用新的路标点更新状态向量 X

文章插图

3.19 更新协方差矩阵
更新协方差矩阵 P: 对角线元素

文章插图

更新协方差矩阵 P: 机器人-路标相关元素

• 顶级风水宝地 至尊蓝月风水宝地
• 各种牡丹寓意和象征风水 蓝色牡丹寓意风水好吗
• 世界十大地理奇观欣赏：伯利兹蓝洞上榜，石浪居第一位
• 富德无线鼠标连接方法 蓝牙鼠标怎么连接电脑呢
• 蓝色钱包真的不好吗 蓝色钱包有风水吗吗
• 为什么有人眼白是淡蓝色 白眼球发蓝图片
• 2022长安cs75蓝鲸版1.5t配6at 长安cs75价格及图片大全新款怎么样
• 江西蓝天学校 江西南昌蓝天学院校长
• 世界罕见的十大天文现象盘点：日全食蓝月亮均上榜
• 看完就知道该咋办了 带的蓝牙入耳式耳机老是滑出来怎么办