背景：一直在使用模板、自动化求导，对于背后雅可比推导只剩一个基本概念，有必要好好梳理巩固一下。本人水平有限，若推导过程有误，欢迎评论区提出。
假设一个二维slam问题，使用欧式距离观测模型，即观测量 x k x{_k} xk是机器人到特征点之间的距离。
问题设定
- 机器人状态 x i x{_i} xi = ( x i , y i , θ i x{_i},y{_i},\theta{_i} xi,yi,θi)，其中( x i , y i x{_i},y{_i} xi,yi)是机器人在时刻 i i i的位置， θ i \theta{_i} θi是机器人的朝向。
- 地图中特征点 m j m{_j} mj = ( m x j , m y j m{_x{_j}},m{_y{_j}} mxj,myj)表示特征点。
- 观测模型假设机器人可以通过传感器（如激光雷达）观测与特征点之间的欧式距离
一、观测模型
机器人从位置( x i , y j x{_i},y{_j} xi,yj)观测到特征点 m j m{_j} mj = ( m x j , m y j ) (m{_x{_j}},m{_y{_j}}) (mxj,myj)的距离。则观测模型可表示为：
h ( x i , m j ) h(x{_i},m{_j}) h(xi,mj) = ( m x j − x i ) 2 + ( m y j − y i ) 2 \sqrt{(m{_x{_j}} - x{_i})^2 + (m{_y{_j}} - y{_i})^2} (mxj−xi)2+(myj−yi)2
二、残差函数
残差函数 e k ( x i , m j ) e{_k}(x{_i},m{_j}) ek(xi,mj)表示观测值与预测值之间的误差，表示为:
e ( x i , m j ) = h ( x i , m j ) − z k e(x{_i}, m{_j})=h(x{_i},m{_j})-z{_k} e(xi,mj)=h(xi,mj)−zk
其中， z k z{_k} zk是传感器观测到的实际距离。
三、雅可比矩阵推导
需要计算的残差 e k ( x i , m j ) e{_k}(x{_i},m{_j}) ek(xi,mj)对机器人状态 x i x{_i} xi和地图中特征点 m j m{_j} mj的偏导数，依次获取雅可比矩阵。
3.1 计算 e k e{_k} ek对机器人状态 x i = ( x i , y i , θ i ) x{_i}=(x{_i},y{_i},\theta{_i}) xi=(xi,yi,θi)的偏导数
残差函数：
e k ( x i , m j ) = ( m x j − x i ) 2 + ( m y j − y i ) 2 − z k e{_k}(x{_i},m{_j})=\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}-z{_k} ek(xi,mj)=(mxj−xi)2+(myj−yi)2−zk
首先， e k e{_k} ek对 x i x{_i} xi和 y i y{_i} yi进行求偏导。根据该式子， θ i \theta{_i} θi在该观测模型并不参与。因此，只需要进行对 x i x{_i} xi和 y i y{_i} yi求偏导数。当然，根据残差函数，对 θ i \theta{_i} θi进行求导数：
∂ e k ∂ θ i = 0 \frac {\partial e{_k}}{\partial \theta{_i}}=0 ∂θi∂ek=0
3.1.1对 x i x{_i} xi求偏导
∂ e k ∂ x i = ∂ ( ( m x j − x i ) 2 + ( m y j − y i ) 2 ) ∂ x i \frac {\partial e{_k}}{\partial x{_i}}=\frac{\partial(\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2})}{\partial x{_i}} ∂xi∂ek=∂xi∂((mxj−xi)2+(myj−yi)2)
利用链式法则和平方根的导数公式：
∂ e k ∂ x i = 1 2 ∗ ( m x j − x i ) 2 + ( m y j − y i ) 2 ∗ 2 ∗ ( x i − m x j ) \frac {\partial e{_k}}{\partial x{_i}}=\frac{1}{2*\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}}*2*(x{_i}-m{_x{_j}}) ∂xi∂ek=2∗(mxj−xi)2+(myj−yi)21∗2∗(xi−mxj)
化简得：
∂ e k ∂ x i = x i − m x j ( m x j − x i ) 2 + ( m y j − y i ) 2 \frac {\partial e{_k}}{\partial x{_i}}=\frac{x{_i}-m{_x{_j}}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}} ∂xi∂ek=(mxj−xi)2+(myj−yi)2xi−mxj
3.1.2 对 y i y{_i} yi求偏导
∂ e k ∂ y i = 1 2 ∗ ( m x j − x i ) 2 + ( m y j − y i ) 2 ∗ 2 ∗ ( y i − m y j ) \frac {\partial e{_k}}{\partial y{_i}}=\frac{1}{2*\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}}*2*(y{_i}-m{_y{_j}}) ∂yi∂ek=2∗(mxj−xi)2+(myj−yi)21∗2∗(yi−myj)
化简得：
∂ e k ∂ y i = y i − m y j ( m x j − x i ) 2 + ( m y j − y i ) 2 \frac {\partial e{_k}}{\partial y{_i}}=\frac{y{_i}-m{_y{_j}}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}} ∂yi∂ek=(mxj−xi)2+(myj−yi)2yi−myj
综合，残差对 x i x{_i} xi的雅可比矩阵 J e k , x i J{_e{_k},{_x{_i}}} Jek,xi矩阵为：
J e k , x i = [ x i − m x j ( m x j − x i ) 2 + ( m y j − y i ) 2 , y i − m y j ( m x j − x i ) 2 + ( m y j − y i ) 2 ] J{_e{_k},{_x{_i}}}=[\frac{x{_i}-m{_x{_j}}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}},\frac{y{_i}-m{_y{_j}}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}}] Jek,xi=[(mxj−xi)2+(myj−yi)2xi−mxj,(mxj−xi)2+(myj−yi)2yi−myj]
3.2 计算 e k e{_k} ek关于地图中特征点 m j = ( m x j , m y j ) m{_j}=(m{_x{_j}},m{_y{_j}}) mj=(mxj,myj)的偏导数
3.2.1 对 m x j m{_x{_j}} mxj求导
∂ e k ∂ m x j = ∂ ( ( m x j − x i ) 2 + ( m y j − y i ) 2 ) ∂ m x j \frac {\partial e{_k}}{\partial m{_x{_j}}}=\frac{\partial(\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2})}{\partial m{_x{_j}}} ∂mxj∂ek=∂mxj∂((mxj−xi)2+(myj−yi)2)
使用链式法则
∂ e k ∂ m x j = 1 2 ∗ ( m x j − x i ) 2 + ( m y j − y i ) 2 ∗ 2 ∗ ( m x j − x i ) \frac {\partial e{_k}}{\partial m{_x{_j}}}=\frac{1}{2*\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}}*2*(m{_x{_j}}-x{_i}) ∂mxj∂ek=2∗(mxj−xi)2+(myj−yi)21∗2∗(mxj−xi)
化简得：
∂ e k ∂ m x j = m x j − x i ( m x j − x i ) 2 + ( m y j − y i ) 2 \frac {\partial e{_k}}{\partial m{_x{_j}}}=\frac{m{_x{_j}}-x{_i}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}} ∂mxj∂ek=(mxj−xi)2+(myj−yi)2mxj−xi
3.2.2 对 m y j m{_y{_j}} myj求偏导
类似计算 ∂ e k ∂ m y j \frac {\partial e{_k}}{\partial m{_y{_j}}} ∂myj∂ek:
∂ e k ∂ m y j = 1 2 ∗ ( m x j − x i ) 2 + ( m y j − y i ) 2 ∗ 2 ∗ ( m y j − y i ) \frac {\partial e{_k}}{\partial m{_y{_j}}}=\frac{1}{2*\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}}*2*(m{_y{_j}}-y{_i}) ∂myj∂ek=2∗(mxj−xi)2+(myj−yi)21∗2∗(myj−yi)
化简得：
∂ e k ∂ m y j = m y j − y i ( m x j − x i ) 2 + ( m y j − y i ) 2 \frac {\partial e{_k}}{\partial m{_y{_j}}}=\frac{m{_y{_j}}-y{_i}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}} ∂myj∂ek=(mxj−xi)2+(myj−yi)2myj−yi
因此，残差 e k e{_k} ek对特征点 m j m{_j} mj的雅可比矩阵 J e k , m j J{_e{_k},_m{_j}} Jek,mj为：
J e k , m j = [ m x j − x i ( m x j − x i ) 2 + ( m y j − y i ) 2 , m y j − y i ( m x j − x i ) 2 + ( m y j − y i ) 2 ] J{_e{_k},_m{_j}}=[\frac{m{_x{_j}}-x{_i}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}},\frac{m{_y{_j}}-y{_i}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}}] Jek,mj=[(mxj−xi)2+(myj−yi)2mxj−xi,(mxj−xi)2+(myj−yi)2myj−yi]
四、雅可比矩阵小结
雅可比矩阵 J e k J{_e{_k}} Jek由两部分组成，一个是对机器人状态 x i x{_i} xi的求偏导，另为一部分是对地图特征点 m j m{_j} mj的求偏导：
J e k = [ J e k , x i , J e k , m j ] J{_e{_k}}=[J{_e{_k},{_x{_i}}},J{_e{_k}},_m{_j}] Jek=[Jek,xi,Jek,mj]
其中：
J e k , x i = [ x i − m x j ( m x j − x i ) 2 + ( m y j − y i ) 2 , y i − m y j ( m x j − x i ) 2 + ( m y j − y i ) 2 ] J{_e{_k},{_x{_i}}}=[\frac{x{_i}-m{_x{_j}}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}},\frac{y{_i}-m{_y{_j}}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}}] Jek,xi=[(mxj−xi)2+(myj−yi)2xi−mxj,(mxj−xi)2+(myj−yi)2yi−myj]
和
J e k , m j = [ m x j − x i ( m x j − x i ) 2 + ( m y j − y i ) 2 , m y j − y i ( m x j − x i ) 2 + ( m y j − y i ) 2 ] J{_e{_k},_m{_j}}=[\frac{m{_x{_j}}-x{_i}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}},\frac{m{_y{_j}}-y{_i}}{\sqrt{(m{_x{_j}-x{_i}})^2+(m{_y{_j}}-y{_i})^2}}] Jek,mj=[(mxj−xi)2+(myj−yi)2mxj−xi,(mxj−xi)2+(myj−yi)2myj−yi]
至此，样例手推雅可比完毕
公式编辑器参考
###################
好记性不如烂笔头
积跬步期千里
觉得不错的话，记得点赞收藏
