學習筆記（優達學城）-無損濾波器 UKF

這次文章打算寫關於無損濾波器的文章。

封面中，我們是在固定感測器的情況下，用lidar和radar跟蹤小車的位置。紅色和藍色點都是感測器的值，而綠色的點就是經過ukf的結果。這裡，要用到非線性濾波的是radar， 因為它的感測器值是需要非線性轉換的。 而lidar則不需要。 指的注意的是，非線性和線性都是相對於我們取的狀態變數而言的。比如，我們的狀態空間里定義的變數恰好就都是從radar 直接不通過任何轉換就可以提取的，那radar在這裡就不需要非線性轉換，而lidar 可能需要。

事實上，我應該先寫經典卡爾曼濾波器Kalman filter 和擴展卡爾曼濾波器Extended kalman filter的。

不過，正好剛完成了無損濾波器unscented kalman filter project。 所以打算先寫完關於UKF的文章，因為現在記憶還比較新鮮。

首先來個代碼傳送門。

Fred159/Udacity-Term2-Project2-UKF?

github.com

首先簡單介紹一下KF,EKF和UKF的區別。

以誕生時間來算的話，KF是大哥，EKF是二哥，UKF是三弟。

以功能來算的話，KF大哥的誕生是為了處理線性遞歸問題。KF大哥的誕生解決了很多以前沒能好好解決的問題，有時也被稱作最優濾波器，從此名震江湖。KF大哥確實是劃時代的濾波器，但是因為KF是解決線性回歸問題的濾波器，他在處理非線性系統的問題上不是很給力。所以，有個叫EKF的二弟總結了KF大哥的優點，並且憑藉偏導數和雅克比矩陣的能力，優化了KF大哥處理非線性問題的性能。不過因為偏導數高階導數省略問題和雅克比矩陣計算難度的問題，讓EKF的效果不是很好。 再然後，三弟UKF就出現了。 UKF按一定距離取sigma points 的方式，然後讓這些sigma points 按照狀態轉移矩陣更新一下sigma points狀態的預測值，再然後根據sigma points 算出權重，最終計算出x的預測值。測量值更新的時候，（需要計算非線性的）再得到感測器信息之前，會再利用預測後的sigma points和權重，根據測量轉移矩陣得到測量值的預測值。這樣測量誤差z_error就能根據z_感測器值 - z預測值得到。之後就比較簡單了， 先計算出correlation matrix，再計算Kalman gain， 在更新x_和P_就可以了（因為他們會在下一次計算的時候用到）。

總的來說。

• KF處理非線性模型
• EKF 通過雅克比和偏導數近似非線性模型，但是忽略了高階導數
• UKF 通過去點的方式近似非線性模型，因為沒有用雅克比和偏導數，讓計算變得更加簡單，同時也沒有忽略高階導數項。所以相對EKF,UKF計算量少，且效果好。唯一的問題就是，看著太複雜。（實際上也複雜，所以我們在下面的文章詳細討論一下）

UKF的理念

跟KF，EKF一樣，UKF也是通過預測，狀態轉移矩陣，預測測量值，更新測量值，再根據預測的測量值和實際的測量值之間的差值求出kalman gain，最後根據kalman gain 獲得準確的狀態向量和協方差。

不一樣，且很不一樣的就是會出現sigma points~！

下圖為UKF的整個演算法的流程。prediction方面有三步，update有兩步，其實還有一部分，我認為要在prediction前面，就是初始化。不過這不重要~

那麼，我們看看UKF的sigma points 為什麼能解決EKF 不能解決的問題。（個人見解，有指導意見，一定洗耳恭聽）

（下圖來自優達學城）

以下這段為個人理解，

知道下個狀態的P（covariance）就意味著我們知道整個數據的誤差範圍是圓的還是橢圓形的還是完全不規則形的。但是，非線性情況下，我們是不能預測下一個狀態裡面的P的大小的。就算有也會像EKF一樣，忽略非線性模型的高階導數的影響，這樣務必會導致P的誤差範圍變大。同時非線性還導致了x的計算本身都有較大的誤差。

所以UKF就選擇了不一樣的方法。它生成了一些點，來近似非線性。由這些點來決定實際X和P的取值範圍。感覺有點像粒子濾波器的概念，但還有些不同，因為UKF里的sigma points的生成並沒有概率的問題。UKF的sigma points 就是把不能解決的非線性單個變數的不確定性，用多個sigma points的不確定性近似了。

UKF具體流程-Prediction-生成sigma points

下面討論如何生成UKF的sigma points。

下圖的公式為生成sigma points的公式。 第一列就是x現在的值，也就是從上一個狀態接手的x值。第二列和第三列的公式中 lambda 是一個數字。具體演算法是，lambda = 3-nx。是個經驗公式。這裡nx就是狀態變數的個數。 如果我們有5個狀態需要測量，那麼nx就等於5。 那麼根據下面公式就可以得到[5x11]的矩陣了。生成的矩陣代表的含義就是，按照一定規律生成了環繞在x周邊的10個點。 由這10個點的平均值定義x的實際值（見下兩圖）。（事實上，lambda 表現的是sigma points離x的距離。）

具體代碼C++ 實現如下。

MatrixXd UKF::GenerateSigmaPoints(VectorXd x, MatrixXd P, double lambda, int n_sig) { int n = x.size(); //create sigma point matrix MatrixXd Xsig = MatrixXd( n, n_sig ); //calculate square root of P MatrixXd A = P.llt().matrixL(); Xsig.col(0) = x; double lambda_plue_n_x_sqrt = sqrt(lambda + n); for (int i = 0; i < n; i++){ Xsig.col( i + 1 ) = x + lambda_plue_n_x_sqrt * A.col(i); Xsig.col( i + 1 + n ) = x - lambda_plue_n_x_sqrt * A.col(i); } return Xsig;}

UKF具體流程-Prediction-生成Augmented sigma points

什麼叫augmented？ augmented 增廣的；增音的；擴張的。 我理解為增加擴展。為什麼叫擴展。因為我們的狀態方程裡面是有雜訊vk的。當這個vk對我們的狀態轉移矩陣有影響的話，我們需要講這個雜訊vk考慮到我們的狀態轉移矩陣裡面的。所以，我們同時也把vk當作一種狀態（雜訊狀態）放進我們的狀態變數空間里。

也就是說，按照上面的介紹中說道，假設原來的狀態變數個數是5個。那麼由於還要顧及vak和v_phik的影響，要把這兩個雜訊也放進狀態變數里。

5個狀態=》7個狀態。 5個原來的狀態+2個雜訊狀態。

其實在寫代碼的時候，直接按照擴展後的狀態寫就可以了

//create augmented mean state x_aug.head(5) = x; x_aug(5) = 0; x_aug(6) = 0; //create augmented covariance matrix P_aug.fill(0.0); P_aug.topLeftCorner(5,5) = P; P_aug(5,5) = std_a*std_a; P_aug(6,6) = std_yawdd*std_yawdd; //create square root matrix MatrixXd L = P_aug.llt().matrixL(); //create augmented sigma points Xsig_aug.col(0) = x_aug; for (int i = 0; i< n_aug; i++) { Xsig_aug.col(i+1) = x_aug + sqrt(lambda+n_aug) * L.col(i); Xsig_aug.col(i+1+n_aug) = x_aug - sqrt(lambda+n_aug) * L.col(i); }

UKF具體流程-Prediction-prediction state

現在我們生成了很多sigma points ，那麼因為物體會按一定規律移動，所以我們需要預測物體的下一個狀態。這裡就是根據狀態轉移矩陣來計算的。 狀態轉移矩陣的構建階段就是，我們對物理現象的建模過程這裡就不多闡述。總是就是一套公式x_k+1 = f(xk,nk)。

本次舉例當中的代碼如下

//predict sigma points for (int i = 0; i< 2*n_aug+1; i++) { //extract values for better readability double p_x = Xsig_aug(0,i); double p_y = Xsig_aug(1,i); double v = Xsig_aug(2,i); double yaw = Xsig_aug(3,i); double yawd = Xsig_aug(4,i); double nu_a = Xsig_aug(5,i); double nu_yawdd = Xsig_aug(6,i); //predicted state values double px_p, py_p; //avoid division by zero if (fabs(yawd) > 0.001) { px_p = p_x + v/yawd * ( sin (yaw + yawd*delta_t) - sin(yaw)); py_p = p_y + v/yawd * ( cos(yaw) - cos(yaw+yawd*delta_t) ); } else { px_p = p_x + v*delta_t*cos(yaw); py_p = p_y + v*delta_t*sin(yaw); } double v_p = v; double yaw_p = yaw + yawd*delta_t; double yawd_p = yawd; //add noise px_p = px_p + 0.5*nu_a*delta_t*delta_t * cos(yaw); py_p = py_p + 0.5*nu_a*delta_t*delta_t * sin(yaw); v_p = v_p + nu_a*delta_t; yaw_p = yaw_p + 0.5*nu_yawdd*delta_t*delta_t; yawd_p = yawd_p + nu_yawdd*delta_t; //write predicted sigma point into right column Xsig_pred(0,i) = px_p; Xsig_pred(1,i) = py_p; Xsig_pred(2,i) = v_p; Xsig_pred(3,i) = yaw_p; Xsig_pred(4,i) = yawd_p; }

總之我們現在用f(x)獲得了很多預測後的sigma points的值。那麼現在就需要整合所有的sigma points計算實際預測的xpred 和 P_pred了。

UKF具體流程-Prediction-Predicted mean and covariance

我們現在有很多預測後的sigma points。那麼我們需要實際預測的xpred 和 P_pred了。

我們按照一定規律去計算個個sigma points的權重，並且根據這個權重計算xpred 和 P_pred。那麼具體公式如下。代入就好。這裡需要解釋就是lambda+na這項在生成sigma points的時候，就出現過。他代表的意思就是，sigma points 距離x越遠，權重就越小。

具體代碼如下。這裡需要注意的就是，weight剛開始的時候，是要被初始化的。且weight的第一個值的計算方法和其他不一樣哦。 因為weight的第一個代表的是我們要找的x值。

// set weights double weight_0 = lambda/(lambda+n_aug); weights(0) = weight_0; for (int i=1; i<2*n_aug+1; i++) { //2n+1 weights double weight = 0.5/(n_aug+lambda); weights(i) = weight; } //predicted state mean x.fill(0.0); for (int i = 0; i < 2 * n_aug + 1; i++) { //iterate over sigma points x = x+ weights(i) * Xsig_pred.col(i); } //predicted state covariance matrix P.fill(0.0); for (int i = 0; i < 2 * n_aug + 1; i++) { //iterate over sigma points // state difference VectorXd x_diff = Xsig_pred.col(i) - x; //angle normalization while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI; while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI; P = P + weights(i) * x_diff * x_diff.transpose() ; }

UKF具體流程-Measurement Prediction-radar

在得到具體的一個數字的x和P的預測值之後，我們需要利用求出來的x預測值來計算z_預測值。z代表測量值。為了計算我們預測的z和實際的感測器數據z的誤差（因為這個要在以後的計算中用到），我們需要通過測量轉移矩陣把狀態空間向量和感測器可得到的數據關聯起來。也就是說，通過狀態空間向量x測量轉移矩陣=z_預測值。下圖中，x_k+1就是我們在上一個步驟中的sigma points。

z_k+1就是我們這裡求的z_預測值。

注意，這裡z_k+1隻是我們計算出來的，並不是來自感測器的數據。

這裡要注意的是，狀態空間向量是7維的，而感測器radar 可讀取的數據為3個，需要注意測量轉移矩陣的大小。

預測值prediction有一定的公式，如下圖。

1. 先利用上一步sigma points預測值（也就是15列sigma points）挨個求出相對應的測量值預測值。這樣我們可以得到15列相應的測量值。這個值就是下圖中的z_k+1|k,i 。
2. 這裡有一次出現了wi 。 wi 就是上一步中用來求x平均值的權重。這裡我們可以不需要額外的計算。
3. 把每一個wi（1x15列）和對應的每一個z_k+1|k,i （3x15列）相乘，最終得到一個值，也就是z的預測值。
4. 這裡w_k+1 是measurement model 裡面的noise。他對系統沒有非線性影響，所以也不用被擴展測量向量空間，只需要簡單的相加就可以了。
5. 然後計算predicted measurement covariance matrix。按照一下公式計算就可以。同樣，因為R也是因為沒有非線性影響，所以也可以直接相加。

//transform sigma points into measurement space for (int i = 0; i < 2 * n_aug + 1; i++) { //2n+1 simga points // extract values for better readibility double p_x = Xsig_pred(0,i); double p_y = Xsig_pred(1,i); double v = Xsig_pred(2,i); double yaw = Xsig_pred(3,i); double v1 = cos(yaw)*v; double v2 = sin(yaw)*v; // measurement model Zsig(0,i) = sqrt(p_x*p_x + p_y*p_y); //r Zsig(1,i) = atan2(p_y,p_x); //phi Zsig(2,i) = (p_x*v1 + p_y*v2 ) / sqrt(p_x*p_x + p_y*p_y); //r_dot }//mean predicted measurement VectorXd z_pred = VectorXd(n_z); z_pred.fill(0.0); for (int i=0; i < 2*n_aug+1; i++) { z_pred = z_pred + weights(i) * Zsig.col(i); } //innovation covariance matrix S MatrixXd S = MatrixXd(n_z,n_z); S.fill(0.0); for (int i = 0; i < 2 * n_aug + 1; i++) { //2n+1 simga points //residual VectorXd z_diff = Zsig.col(i) - z_pred; //angle normalization while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI; while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI; S = S + weights(i) * z_diff * z_diff.transpose(); } //add measurement noise covariance matrix MatrixXd R = MatrixXd(n_z,n_z); R << std_radr*std_radr, 0, 0, 0, std_radphi*std_radphi, 0, 0, 0,std_radrd*std_radrd; S = S + R;

UKF具體流程-Measurement Prediction-Update radar

這是UKF的最後階段。這裡我們最終根據x預測值，和z預測值求出kalman gain和cross-correlation function然後最終更新state 和協方差。都是單純的計算。而且state update和covariance matrix update都是跟KF一樣的. UKF獨有的計算有kalman gain和cross-correlation function。不過也都是單純的計算。這裡我想說一下，我理解的cross-correlation function的作用。 通過式子我們可以看出求cross-correlation function的內部結構。他需要每個預測的sigma points 和x預測值的差和每個sigma point 預測的測量值和z預測值的差 。也就是說，cross-correlation 這個方程會根據sigma points和預測值之間的差來平衡kalman gain。 而kalman gain裡面不知用到cross-correlation function，還會用到測量值協方差預測值。這樣kalman gain就可以利用每個預測的sigma points 和x預測值的差和每個sigma point 預測的測量值和z預測值的差，來平衡模型的預測準確度和感測器的預測準確度。（kalman gain 就是一種權重）

代碼實現如下

//calculate cross correlation matrix Tc.fill(0.0); for (int i = 0; i < 2 * n_aug + 1; i++) { //2n+1 simga points //residual VectorXd z_diff = Zsig.col(i) - z_pred; //angle normalization while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI; while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI; // state difference VectorXd x_diff = Xsig_pred.col(i) - x; //angle normalization while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI; while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI; Tc = Tc + weights(i) * x_diff * z_diff.transpose(); } //Kalman gain K; MatrixXd K = Tc * S.inverse(); //residual VectorXd z_diff = z - z_pred; //angle normalization while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI; while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI; //update state mean and covariance matrix x = x + K * z_diff; P = P - K*S*K.transpose();

總結

事實上，這個project 是融合lidar 和radar的演算法。說是融合，但說白了就是，不同時間段處理不同感測器input而已。因為這個input的不一樣決定了，裡面運行的代碼是ukf還是ekf。像在這個project 裡面，lidar 是線性的，所以就不需要UKF ，而radar因為感測器獲取的數據種類就要求了它要用UKF來實現（當然EKF也可以，但是精度低而已）。

這裡就不詳細闡述了。 具體參考我的github代碼。

開發環境

開發環境把， 我是這麼做的。 也是開個傳送門。這是我用docker來完成udacity 的project的。大家如果不懂可以參考一下。其實剛開始還是挺麻煩的。