在之前的《slam中的eskf推導》一文中，沒有寫觀測矩陣 H 矩陣的過程，現在補上這部分。

前置

列舉幾個等下推導需要用到的一些點：

平面特征點構造觀測矩陣

例如在 fastlio 中，是利用平面特征點到擬合平面的距離來構造觀測方程，利用平面特征點到擬合平面的距離應該趨向于0來列方程。假設擬合后的法向量為 u = [A, B, C]'，截距為 D，點到平面的距離為 d = Ax + By + Cz + D。

以距離來列觀測方程：

對于 q 點的解釋

對于平面方程，可以寫作：

u'(x - q) = 0

u 是單位法向量，q 是平面上的任意已知點，點到平面的距離可以寫作：

d = u'(x - q)

如果 x 也在平面上，則 d = 0

q 的選取不影響距離值的計算，因為：

u'(x - q1) = u'(x - q2)

推導觀測方程

添加姿態偏差 δR，平移偏差 δt，以及噪聲 V：

變換式子，可以得到：

等式兩邊對 δx 求偏導，得：

之后對 δx 中的角度 δθ 和 位移 δt 分別求偏導，得到：

對 δt 求偏導就比較簡單了：

那么對于點 Pi 以及整體的觀測矩陣 H：