各传感器分析

imu

对于平面移动机器人（如扫地机器人），IMU只需要一般只需要使用陀螺仪的偏航角（YAW），陀螺仪的偏航角有时间漂移的误差存在，一般分为系统漂移误差（offset）和随机时间漂移误差。

轮式编码器

没啥好说的，两轮差分机器人有对应的运动模型，累计误差随着打滑等因素逐步变大。需要把轮径和两轮中心距标定出来，标定的方法比较多（todo:更新三种标定方法）。

参考论文：

Ego- and object motion estimation

论文中的工程：

GitHub - tum-phoenix/drive_ros_localize_odom_fusion: Fuses odometry message from various sources

mediaTUM - Media and Publication Server

光电鼠标传感器

可以测出来xy偏移量的，放在机器人不同的位置和放法是有讲究的。

卡尔曼融合滤波

预测模型

模型就是两轮差分轮的模型，这里要做一个偏导，算出雅可比矩阵，即状态转移矩阵F，也就是这里体现了非线性，其他按照卡尔曼滤波算法的流程走就可。

起始模型方差P0设为0.1×单位矩阵，过程噪声Q从小往大了调。

观测模型

todo

搞个状态变换矩阵H出来

测量噪声协方差R根据静止/直线运动和圆周运动统计出来，算出来×9倍（按照3sigma高斯噪声模型来处理）