背景：一直在使用模板、自动化求导，对于背后雅可比推导只剩一个基本概念，有必要好好梳理巩固一下。本人水平有限，若推导过程有误，欢迎评论区提出。
假设一个二维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]
至此，样例手推雅可比完毕
公式编辑器参考
###################
好记性不如烂笔头
积跬步期千里
觉得不错的话，记得点赞收藏