Sensor

對於無人駕駛系統而言，多傳感器已經是默認配置

Kalman Filter 卡爾曼濾波

Kalman Filter 經常運用於無人駕駛系統中感知模塊，用於目標狀態估計。用人話說，就是物體追蹤。

簡單來說，有個運動的小車，用來測量小車運動的傳感器其實有測量噪聲 (Measurement Noise)，所以得到的結果是個高斯分佈

如果我們用帶誤差的測量值來預測下一時刻的位置，由於加入了速度估計噪聲，所以不確定性更大了

於是用傳感器再做一次測量，新的測量依然帶有誤差(還是個高斯分佈)

將得到的兩個高斯分佈加權取平均，得到新的高斯分佈(綠)

這步操作中使用到的加權數值叫做卡爾曼增益，決定了我們對當前測量的信任程度。新得到的綠色高斯分佈，擁有比前兩次測量值更小的方差。說明卡爾曼濾波從兩個不確定較高的分佈，得到了一個相對確定的分佈。並且新的高斯分佈可以作爲下次預測的初始值(卡爾曼濾波假設本次測量只和上次測量有關)，所以卡爾曼濾波可以迭代。

小車插圖圖源《Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation》

多傳感融合 Lidar and Radar Fusion

一個簡單的感知反饋模型其實只有兩步：狀態預測與測量更新

在多傳感器條件下，各傳感器之間想要同步反饋速度其實並無必要。每個傳感器異步地參與感知反饋

在任何時間，只要收到傳感器的數據，就會觸發一次測量更新

這個過程中，會一直使用KF預測和KF更新

狀態預測 State Prediction

線性模型假設，物體在運動時，每段時間間隔中速度恆定。實際上，每次測量時間之間的間隔是不定的，物體的加速也是不定的

時間和加速度的不確定性決定了過程噪聲process Noise

下圖引入了狀態轉移矩陣

過程協方差矩陣 Process Covariance Matrix

由於狀態向量只包含位置和速度信息，實際上加速度在模型中是作爲隨機噪聲的

由於加速度不確定，所以直接當成隨機成分。然後對上面的式子求導，就得到下面的隨機加速向量v

v是服從於N(0, Q)分佈

再把v分解成兩個矩陣。一個4x2的矩陣G，其中不包含隨機變量。一個2x1的矩陣a，包含隨機加速項。

根據定義，協方差矩陣Q又是v乘上v的轉置的數學期望。由於G不包含隨機項，所以移到了數學期望的外面

剩下的就是ax的方差，ay的方差，ax和ay的協方差。由於ax和ay不相關，所以協方差是0. 前前後後放到一起，就是下面這玩意兒

激光檢測 Laser Measurement

使用激光傳感器，獲取點雲數據，探測物體。利用卡爾曼濾波進行轉檯預測

預測效果如下

選取近似直線部分放大，發現預測跟實際物體運行高度吻合

選取轉向部分放大，預測偏離實際運行軌跡。因爲在每個小的時間間隔中，我們一直假設方向不變

雷達檢測 Radar Measurement

激光可以獲得車輛的位置信息，要完成傳感融合還需要從雷達獲取速度信息

雷達信息包含了三個變量：Range範圍(與路人的距離); Bearing方位(從x軸開始逆時針轉向路人方向的角度); Radial Velocity是車速在行人方向上的速度分量，也叫range rate

測量函數h(x')會將測量空間投射到預測空間

Extended Kalman Filter 擴展卡爾曼濾波

EKF(Extended Kalman Filter）是卡爾曼濾波的非線性版本。

原本的卡爾曼濾波假設被Prediction(Estimate)服從高斯分佈，且Measurement(Noisy)也服從高斯分佈。但是現實狀態中，基本都是非線性變換（簡單說就是因爲現實中被測物體多半處於受力狀態，比如摩擦力）。