更新雅可比矩阵的非线性部分笔记
// 更新雅可比矩阵的非线性部分
df(1,6) = -vel_trans * sin(yaw);
df(2,6) = vel_trans * cos(yaw);
// ∂x/∂θ = -v*sin(θ) df(2,6) = vel_trans * cos(yaw);
// ∂y/∂θ = v*cos(θ) 为什么是这两行 而不是别的行呢
我们重点分析为什么在雅可比矩阵中只更新了(1,6)和(2,6)这两个位置。这需要从机器人的运动模型和状态向量的定义入手。
状态向量（state）的定义：
1: x（机器人在世界坐标系中的x坐标）
2: y（机器人在世界坐标系中的y坐标）
3: z
4: roll
5: pitch
6: yaw（机器人在世界坐标系中的偏航角）
控制输入（vel）的定义：
1: 线速度v（沿机器人前进方向，即x方向）
2: 角速度ω
运动模型（状态转移函数）：
x_{k+1} = x_k + v * cos(yaw_k) * Δt
y_{k+1} = y_k + v * sin(yaw_k) * Δt
yaw_{k+1} = yaw_k + ω * Δt
注意：在代码中，传递给运动模型的控制输入（vel）已经包含了时间间隔？实际上，在代码中，我们注意到vel_trans是直接用的vel(1)，而并没有另外乘以Δt。这意味着在调用此运动模型之前，速度应该已经乘以了时间（即实际上传递的是位移和角度变化量）。因此，我们在代码中看到：
state(1) += cos(state(6)) * vel(1); // 相当于Δx = v * cos(θ) * Δt
state(2) += sin(state(6)) * vel(1); // 相当于Δy = v * sin(θ) * Δt
state(6) += vel(2); // 相当于Δθ = ω * Δt
现在，我们需要计算状态转移函数关于状态向量的雅可比矩阵（即偏导数矩阵）F，用于扩展卡尔曼滤波的协方差传播。
雅可比矩阵F的元素定义为：
F(i,j) = ∂f_i / ∂x_j (f_i是状态转移函数第i个分量，x_j是状态向量的第j个分量)
由于运动模型只改变了x, y和yaw（状态向量的第1、2、6个分量），其他分量保持不变（即f_i = x_i，当i=3,4,5时），所以雅可比矩阵中对应于z,roll,pitch的行应该是单位矩阵（对角线上为1，其余为0）。
而我们需要计算的是状态转移函数相对于当前状态（k时刻）的偏导数。具体来看：
-
对于x_{k+1} = x_k + v * cos(yaw_k) * Δt
对x_k的偏导：∂(x_{k+1})/∂x_k = 1
对y_k的偏导：0
对yaw_k的偏导：∂(x_{k+1})/∂yaw_k = -v * sin(yaw_k) * Δt （注意：这里v*Δt就是vel(1)） -
对于y_{k+1} = y_k + v * sin(yaw_k) * Δt
对y_k的偏导：1
对yaw_k的偏导：∂(y_{k+1})/∂yaw_k = v * cos(yaw_k) * Δt -
对于yaw_{k+1} = yaw_k + ω * Δt
对yaw_k的偏导：1 -
其他状态分量的偏导：保持不变，所以对角线上为1，其他为0。
因此，雅可比矩阵F（6x6）为：
x y z r p yaw x [ 1 0 0 0 0 -v*sin(yaw)*Δt ] y [ 0 1 0 0 0 v*cos(yaw)*Δt ] z [ 0 0 1 0 0 0 ] r [ 0 0 0 1 0 0 ] p [ 0 0 0 0 1 0 ]
yaw [ 0 0 0 0 0 1 ]
在代码中，df初始化为单位矩阵（对角线上为1，其他为0）。因此，我们只需要修改第1行第6列（即df(1,6)）和第2行第6列（即df(2,6)）这两个元素，将初始的0改为计算得到的值。
所以代码中：
df(1,6) = -vel_trans * sin(yaw); // 对应∂(x_{k+1})/∂yaw_k = -vΔtsin(yaw)
df(2,6) = vel_trans * cos(yaw); // 对应∂(y_{k+1})/∂yaw_k = vΔtcos(yaw)
而其他位置的偏导数已经是0（非对角线）或者1（对角线），无需改变。
结论：我们只更新了这两个位置，因为运动模型中只有x和y的更新方程含有yaw角（状态向量的第6个分量），其他状态变量没有交叉耦合。
注意：在控制理论中，我们通常将状态转移函数关于状态向量求偏导。这里我们只对状态向量（k时刻）求导，控制输入（vel）在这里视为已知量，不需要求导（因为控制输入是独立的输入，不是状态向量的一部分）。
