2.3运动学演示案例：圆周运动

        以实际案例来演示四元数与旋转矩阵在角速度上的处理方法与差异。用代码来模拟出开车时将方向盘达到固定角度，车辆会画出一个圆周运动的轨迹。我们取前左上作为坐标系，车辆的角速度应该指向Z方向，而固定速度可以用.

        建议使用gitee中的工程，作者在开源项目中描述了如何运行文件以及会遇到哪些问题如何解决，对小白来说能少走很多弯路：slam_in_autonomous_driving: 自动驾驶与机器人中的SLAM技术 从理论到实践配套代码

//
// Created by xiang on 22-12-29.
//

#include <gflags/gflags.h>//用于定义命令行参数
#include <glog/logging.h>//用于日志输出

#include "common/eigen_types.h"
#include "common/math_utils.h"//所使用的数学公式库
#include "tools/ui/pangolin_window.h"//用于可视化界面
//前期到这里要配置好环境，不然会有很多报错，按照gitee中配置不会出任何问题

/// 本节程序演示一个正在作圆周运动的车辆
/// 车辆的角速度与线速度可以在flags中设置

DEFINE_double(angular_velocity, 10.0, "角速度（角度）制");//定义角速度 
DEFINE_double(linear_velocity, 5.0, "车辆前进线速度 m/s");//定义线速度
DEFINE_bool(use_quaternion, false, "是否使用四元数计算");//是否使用四元数更新旋转

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);//初始化Glog日志系统
    FLAGS_stderrthreshold = google::INFO;//设置日志级别（INFO以上输出到stderr）
    FLAGS_colorlogtostderr = true;//日志输出带颜色
    google::ParseCommandLineFlags(&argc, &argv, true);//解析命令行参数

    // 初始化Pangolin可视化窗口
    sad::ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }
// 将角度制角速度转化为弧度制
    double angular_velocity_rad = FLAGS_angular_velocity * sad::math::kDEG2RAD;  
    SE3 pose;                                                                   // 位姿（TWB：世界系到车体系的变换，包含旋转SO3和平移Vec3d）
    Vec3d omega(0, 0, angular_velocity_rad);                                     // 角速度矢量（绕车体z轴旋转，即平面内旋转）
    Vec3d v_body(FLAGS_linear_velocity, 0, 0);                                   // 本体系速度（沿车体x轴前进）
    const double dt = 0.05;                                                      // 每次更新的时间，时间步长（50ms更新一次）
// 再循环中不断更新物体位姿并刷新可视化界面，直到用户退出
    while (ui.ShouldQuit() == false) {
        // 1.更新自身位置
        // 将本体系速度转换到世界系：v_world = R * v_body（R是当前旋转矩阵）
        Vec3d v_world = pose.so3() * v_body;
        // 位置 = 位置 + 世界系速度 * 时间步长（简单欧拉积分）
        pose.translation() += v_world * dt;

        //2. 更新自身旋转
        // 用四元数更新旋转：小角度近似 q_new = q_old * [1, 0.5*ω*dt]
        if (FLAGS_use_quaternion) {
// 用SO3指数映射更新旋转：R_new = R_old * exp(ω*dt)
        // 其中exp(ω*dt)是旋转向量到旋转矩阵的转换（Rodrigues公式）
            Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
            q.normalize();//四元数归一化（避免数值误差）
            pose.so3() = SO3(q);//转换为SO3旋转
        } else {
        // 用SO3指数映射更新旋转：R_new = R_old * exp(ω*dt)
        // 其中exp(ω*dt)是旋转向量到旋转矩阵的转换（Rodrigues公式）
            pose.so3() = pose.so3() * SO3::exp(omega * dt);
        }
        // 3. 输出日志（当前位置）
        LOG(INFO) << "pose: " << pose.translation().transpose();
        // 4. 更新可视化界面（将位姿和速度传给UI）
        ui.UpdateNavState(sad::NavStated(0, pose, v_world));
         // 等待dt时间（模拟实时更新）
        usleep(dt * 1e6);
    }

需要改变参数或者计算方式，填写它的GFlags即可：

如bin/motion --use_quaternion=true（使用四元数） --angular_velocity=15（角速度是15）

ps:括号里面的内容不用写到终端里面

运行结果：

2.4滤波器与最优化理论

2.4.1状态估计问题与最小二乘法

        SLAM问题、定位问题或者建图问题都可以概括为状态估计问题。典型的离散时间状态估计问题由一组运动方程和一组观测问题组成：

2.4.2卡尔曼滤波器

        卡尔曼滤波器描述了如何从一个时刻的状态估计递推到下一个时刻。它由预测和更新这两步骤组成。预测步骤对运动方程进行递推，更新步骤则对上一步的结果进行修正。

此处理解建议去B站观看：（小白入门）从放弃到精通！卡尔曼滤波从理论到实践~_哔哩哔哩_bilibili

（详细版）【卡尔曼滤波器】6_扩展卡尔曼滤波器_Extended Kalman Filter_哔哩哔哩_bilibili

笔记：

2.4.3非线性系统的处理方法

        在非线性系统中，我们首选的方式是对和进行线性化。线性化的本质是求一个函数在固定点的泰勒展开，并保留一阶系数。如果对一个普通的矢量函数在点进行线性化，应该可以得到：

这里的称为雅可比矩阵，H为海塞矩阵，是线性化中最重要的两个矩阵。

        在非线性系统中，可以对运动方程和观测方程进行线性化，然后将卡尔曼滤波器的结论应用在非线性系统中，为扩展卡尔曼滤波器（EKF）,推算过程如下：

2.4.4最优化方法与图优化

        在SLAM领域，最优化问题用图模型进行描述，对应的图模型称为图优化(GraphOptimiza-tion)或者因子图(FactorGraph)。因子图模型可以进一步引入概率图模型中的方法进行求解。

ps：本人对这个最优化方法和图优化的在实际工程中的应用还不太理解，等俺们再学学继续更新，本章最大的感悟是找对学习资源真的是事半功倍呀