擴展卡爾曼濾波（EKF）理論講解與實例（matlab、python和C++代碼）

擴展卡爾曼濾波（EKF）理論講解與實例（matlab、python和C++代碼）

我們上篇提到的卡爾曼濾波是用於線性系統，預測（運動）模型和觀測模型是在假設高斯和線性情況下進行的。簡單的卡爾曼濾波必須應用在符合高斯分佈的系統中，但是現實中並不是所有的系統都符合這樣 。另外高斯分佈在非線性系統中的傳遞結果將不再是高斯分佈。那如何解決這個問題呢？擴展卡爾曼濾波就是幹這個事的。

理論講解

擴展卡爾曼濾波（Extended Kalman Filter，EKF）通過局部線性來解決非線性的問題。將非線性的預測方程和觀測方程進行求導，以切線代替的方式來線性化。其實就是在均值處進行一階泰勒展開。

數學中，泰勒公式是一個用函數在某點的信息描述其附近取值的公式（ 一句話描述：就是用多項式函數去逼近光滑函數 ）。如果函數足夠平滑的話，在已知函數在某一點的各階導數值的情況之下，泰勒公式可以用這些導數值做係數構建一個多項式來近似函數在這一點的鄰域中的值。泰勒公式還給出了這個多項式和實際的函數值之間的偏差。
$f(x)_{Taylor} = \sum_{n=0}^{\infty} \frac{f^{(n)}(a)}{n!} \times (x - a)^n\\ =f(a) + \frac{f'(a)}{1!}(x-a) + \frac{f^{(2)}(a)}{2!}(x-a)^2+ \cdots + \frac{f^{(n)}(a)}{n!}(x-a)^n + R_n(x)$
$f^{(n)}(a)$ 表示 $f(x)$ 在第 $n$階導數的表達式，帶入一個值$a$計算後得到的結果（注意，它是個值）
$\frac{1}{n!}$是一個係數（一個值），每一項都不同，第一項 $\frac{1}{1}$，第二項$\frac{1}{2}$ …… 依此類推
$(x−a)^n$是一個以$x$爲自變量的表達式 。$Rn(x)$是泰勒公式的餘項，是 $(x−a)^n$ 的高階無窮小

KF和EKF模型對比

首先，讓卡爾曼先和擴展卡爾曼濾波做一個對比。在對比過程中可以看出，擴展卡爾曼是一個簡單的非線性近似濾波算法，指運動或觀測方程不是線性的情況,在預測模型部分，擴展卡爾曼的預測模型和量測模型已經是非線性了。爲了簡化計算，EKF通過一階泰勒分解線性化運動、觀測方程。KF與EKF具有相同的算法結構，都是以高斯形式描述後驗概率密度的，通過計算貝葉斯遞推公式得到的。最大的不同之處在於，計算方差時，EKF的狀態轉移矩陣（上一時刻的狀態信息$k-1|k-1$）和觀測矩陣（一步預測$k|k-1$）都是狀態信息的雅克比矩陣（ 偏導數組成的矩陣）。 在預測公式部分，擴展卡爾曼濾波的$F_k$爲$f$的雅可比矩陣，在更新公式部分，擴展卡爾曼濾波的$H_k$爲$h$的雅可比矩陣。

預測模型：
$\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter\\ x_k=Fx_{k-1}+Bu_k+w_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad x_k=f(x_{k-1},u_k)+w_k\\ \vec{z_k}=Hx_k+v_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \vec{z_k}=h(x_k)+v_k$
預測公式：

$\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter\\ \hat x_k=F_k \hat x_{k-1} + B_k \vec u_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \hat x_k=f(x_{k-1},u_k)\\ P_k=F_kP_{k-1}F^T_k+Q_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad P_k=F_kP_{k-1}F^T_k+Q_k$
更新公式：

$\quad Kalman \quad\quad Filter \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad Extended \quad Kalman \quad Filter \\ K' = P_kH^T_k(H_kP_kH^T_k+R_k)^{-1} \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad K' = P_kH^T_k(H_kP_kH^T_k+R_k)^{-1} \\ \hat x'_k = \hat x_k+K'(\vec {z_k}-H_k\hat x_k) \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad \hat x'_k = \hat x_k+K'(\vec {z_k}-h(\hat x_k)) \\ P'_k=P_k-K'H_kP_k \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad P'_k=P_k-K'H_kP_k$

其中
$\hat{x_k}$爲$k$時刻的狀態向量；

$F_k$爲狀態轉移矩陣，表示將$k-1$時刻的狀態向量轉移至t時刻的狀態向量；

$B_k$是輸入控制矩陣，代表着將控制向量 $\hat{u_k}$映射到狀態向量上，統一控制向量 $\hat{u_k}$和狀態向量之間 $\hat{x_k}$的關係；

$\hat{u_k}$代表着控制向量，如加速度，角加速度等；

$w_k$爲過程演化噪聲；

$v_k$爲量測噪聲 ；

$P_k$爲狀態向量的協方差矩陣，代表着狀態向量每個元素之間的關係；

$Q_k$表示預測狀態的高斯噪聲的協方差陣，它用來衡量模型的準確度，模型越準確其值越小；

$\vec {z_k}$爲傳感器測量值的狀態向量，也就是傳感器的測量結果；

$H_k$ 爲轉換矩陣，他將狀態向量 $\hat{x_k}$映射到測量值所在的向量空間$\vec {z_k}$；

$R_k$爲測量值的高斯噪聲的協方差陣，代表着傳感器測量的誤差。

圖片示例：

$J_A,J_H$均爲雅可比矩陣。

雅可比矩陣計算

雅可比矩陣（Jacobian matrix ）是該函數$f(x)$ 的所有行數（$m$個）對狀態向量$\vec x_k$的所有分量（$n$ 個）的一階偏導數組成的矩陣。雅可比矩陣$F_k$的計算如下，其中輸入噪聲爲0。
$x_k=f(x_{k-1},u_k，w_k) \approx f(\hat x_{k-1},u_k,0)+ \underbrace{\left.\frac{\partial {f}}{\partial {x}_{k-1}}\right|_{\hat{{x}}_{k-1}, {u}_{k},0} }_{\mathbf{F}_{k-1}} \left({x}_{k-1}-\hat{{x}}_{k-1}\right) + \underbrace{\left.\frac{\partial {f}}{\partial {w}_{k-1}}\right|_{\hat{{x}}_{k-1}, {u}_{k},0} }_{\mathbf{L}_{k-1}} w_k\\令k=k-1\\F_k\stackrel{def}{=}\frac{\partial f(x)}{\partial x}\stackrel{def}{=} \left[ \begin{array}{ccc} \frac{\partial f}{\partial x_1} & ... & \frac{\partial f}{\partial x_n} \\ \end{array} \right] \stackrel{def}{=} \left[ \begin{array}{ccc} \frac{\partial f_1}{\partial x_1} & ... & \frac{\partial f_1}{\partial x_n} \\ \vdots & \vdots & \vdots \\\frac{\partial f_m}{\partial x_1} & ... & \frac{\partial f_m}{\partial x_n} \end{array} \right] \stackrel{def}{=} J_A.\\$

函數$f(x_{k-1},u_k,w_k)$有$f_1,...,f_m$ 行個分量，於是有$m$行。狀態向量$\vec x_k$有$x_1,...,x_n$個分量，於是有$n$列。

雅可比矩陣$H_k$的計算如下，其中輸入噪聲爲0。
$y_k=h(x_{k},v_k) \approx h(\hat x_{k},0)+ \underbrace{\left.\frac{\partial {h}}{\partial {x}_{k}}\right|_{\hat{{x}}_{k}, 0} }_{\mathbf{H}_{k}} \left({x}_{k}-\hat{{x}}_{k}\right) + \underbrace{\left.\frac{\partial {h}}{\partial {v}_{k}}\right|_{\hat{{x}}_{k}, 0} }_{\mathbf{M}_{k}} v_k \\H_k\stackrel{def}{=}\frac{\partial h(x)}{\partial x}\stackrel{def}{=} \left[ \begin{array}{ccc} \frac{\partial h}{\partial x_1} & ... & \frac{\partial h}{\partial x_n} \\ \end{array} \right] \stackrel{def}{=} \left[ \begin{array}{ccc} \frac{\partial h_1}{\partial x_1} & ... & \frac{\partial h_1}{\partial x_n} \\ \vdots & \vdots & \vdots \\ \frac{\partial h_m}{\partial x_1} & ... & \frac{\partial h_m}{\partial x_n} \end{array} \right] \stackrel{def}{=} J_H.$

函數$h(x_{k},v_k)$有$h_1,...,h_m$ 行個分量，於是有$m$行。狀態向量$\vec x_k$ 有$x_1,...,x_n$個分量，於是有$n$列。

矩陣計算實例

舉例說明：$f(x)$有$f_1,f_2$兩個函數，狀態向量$\vec x_k$有$x_1,x_2$兩個分量，所以有
${f}({x})=\left[\begin{array}{l} f_{1} \\ f_{2} \end{array}\right]=\left[\begin{array}{c} x_{1}+x_{2} \\ x_{1}^{2} \end{array}\right] \quad \quad \frac{\partial {f}}{\partial {x}}=\left[\begin{array}{cc} \frac{\partial f_{1}}{\partial x_{1}} & \frac{\partial f_{1}}{\partial x_{2}} \\ \frac{\partial f_{2}}{\partial x_{1}} & \frac{\partial f_{2}}{\partial x_{2}} \end{array}\right]=\left[\begin{array}{cc} 1 & 1 \\ 2 x_{1} & 0 \end{array}\right]$
又如：

應用實例

線性模型

常用的線性模型有

• 恆定速度模型（Constant Velocity, CV）
• 恆定加速度模型（Constant Acceleration, CA）

這些線性運動模型假定目標是直線運動的，並不考慮物體的轉彎。

CV模型：

CV模型的狀態空間可以表示爲：
$\overrightarrow{x}(t) = (x \quad y \quad v_x \quad v_y)^T$
那麼轉移函數爲：
$\overrightarrow{x}(t+\Delta t) = \left( \begin{array}{c} x(t) + \Delta t v_x \\ y(t) + \Delta t v_y \\ v_x \\ v_y \end{array} \right)$

或者，CV模型的完整的狀態空間還可以表示爲：

$\overrightarrow{x}(t) = (x \quad y \quad v_x \quad v_y \quad a_x \quad a_y)^T$
矩陣表示爲：
$\overrightarrow{x1} =\left( \begin{array}{c} x_1\\ y_1\\ v_{x1}\\ v_{y1}\\ a_{x1}\\ a_{y1} \end{array} \right)= F \cdot \overrightarrow{x0}= \left( \begin{array}{c} 1 & 0 & t & 0 & 0 &0\\ 0 & 1 & 0 & t & 0 &0\\ 0 & 0 & 1 & 0 & 0 &0\\ 0 & 0 & 0 & 1 & 0 &0\\ 0 & 0 & 0 & 0 & 0 &0 \\ 0 & 0 & 0 & 0 & 0 &0 \end{array} \right) \cdot \left( \begin{array}{c} x_0\\ y_0\\ v_{x0}\\ v_{y0}\\ a_{x0}\\ a_{y0} \end{array} \right)= \left( \begin{array}{c} x_0+v_{x0}t\\ y_0+v_{y0}t\\ v_{x0}\\ v_{y0}\\ 0\\ 0 \end{array} \right)$

CA模型

CA模型的狀態空間可以表示爲：

$\overrightarrow{x}(t) = (x \quad y \quad v_x \quad v_y \quad a_x \quad a_y)^T$
矩陣表示爲：
$\overrightarrow{x1} =\left( \begin{array}{c} x_1\\ y_1\\ v_{x1}\\ v_{y1}\\ a_{x1}\\ a_{y1} \end{array} \right)= F \cdot \overrightarrow{x0}= \left( \begin{array}{c} 1 & 0 & t & 0 & \frac{1}{2}t^2 &0\\ 0 & 1 & 0 & t & 0 &\frac{1}{2}t^2\\ 0 & 0 & 1 & 0 & t &0\\ 0 & 0 & 0 & 1 & 0 &t\\ 0 & 0 & 0 & 0 & 1 &0 \\ 0 & 0 & 0 & 0 & 0 &1 \end{array} \right) \cdot \left( \begin{array}{c} x_0\\ y_0\\ v_{x0}\\ v_{y0}\\ a_{x0}\\ a_{y0} \end{array} \right)= \left( \begin{array}{c} x_0+v_{x0}t+\frac{1}{2}a_{x0}t^2\\ y_0+v_{y0}t+\frac{1}{2}a_{y0}t^2\\ v_{x0}+a_{x0}t\\ v_{y0}+a_{y0}t\\ a_{x0}\\ a_{y0} \end{array} \right)$

CA和CV的交互式多模型可以參考我的這篇博客：交互式多模型 IMM的原理及代碼實現（matlab）

非線性模型

非線性模型包括：

• 恆定轉率和速度模型（Constant Turn Rate and Velocity，CTRV）
• 恆定轉率和加速度模型（Constant Turn Rate and Acceleration，CTRA）

CTRV目前多用於機載追蹤系統（飛機），這些二次運動模型大多假定速度 $v$和 偏航角速度（yaw rate） $ω$沒有關係，因此，在這類運動模型中，由於偏航角速度測量的擾動（不穩定），即使車輛沒有移動，我們的運動模型下的角速度也會發生細微的變化。爲了解決這個問題，速度 $v$ 和 偏航角速度 $ω$ 的關聯可以通過設定轉向角 $Φ$恆定來建立，這樣就引出了 恆定轉向角和速度模型（Constant Steering Angle and Velocity，CSAV）, 另外，速度可以別假定爲線性變化的，進而引出了常曲率和加速度模型（Constant Curvature and Acceleration，CCA）。

CTRV模型：

CA、CV模型並不能預測預測轉彎，爲了解決這個問題我們使用CTRV模型:

在CTRV中，使用位置$p_x,p_y$,速度$v$，角度$\psi$，及角速度$\dot{\psi}$來作爲目標的狀態向量：
$\overrightarrow{x}(t) = (p_x \quad p_y \quad v \quad \psi \quad \dot{\psi})^T$
其中，$\psi$爲偏航角，是追蹤的目標車輛在當前車輛座標系下與$x$軸的夾角，逆時針方向爲正，取值範圍是$[0, 2π)$, $\dot{\psi}$是偏航角速度。
其推導過程爲：


CTRV的狀態向量也可以表示爲：

$\overrightarrow{x}(t) = (x \quad y \quad v \quad \theta \quad \omega)^T$
其中，$θ$爲偏航角，$ω$是偏航角速度。
CTRV的狀態轉移函數爲：
$\overrightarrow{x}(t+\Delta t) = \left( \begin{array}{c} \frac{v}{\omega} \sin(\omega \Delta t + \theta) - \frac{v}{\omega}\sin(\theta) + x(t) \\ -\frac{v}{\omega} \cos(\omega \Delta t + \theta) + \frac{v}{\omega}\cos(\theta) + y(t) \\ v \\ \omega \Delta t + \theta \\ \omega \end{array} \right)$
使用CTRV還存在一個問題，那就是 $ω=0$ 的情況，此時我們的狀態轉移函數公式中的$(x,y)$將變成無窮大。爲了解決這個問題，我們考察一下$ω=0$ 的情況，此時我們追蹤的車輛實際上是直線行駛的，所以我們的 $(x,y)$的計算公式就變成了：
$x(t+\Delta t) = v \cos(\theta)\Delta t + x(t)\\ y(t+\Delta t) = v \sin(\theta)\Delta t + y(t)$
那麼現在問題來了，我們知道，卡爾曼濾波僅僅用於處理線性問題，那麼很顯然我們現在的處理模型是非線性的，這個時候我們就不能簡單使用卡爾曼濾波進行預測和更新了，此時預測的第一步變成了如下非線性函數：
$x_{k+1} = f(x_k, u_k)$
其中，$f(x)$表示CTRV運動模型的狀態轉移函數，$u_k$ 表示處理噪聲。爲了解決非線性系統下的問題，我們引入擴展卡爾曼濾波（Extended Kalman Filter，EKF）

CTRV示例

應用場景：

假設我們有激光雷達和雷達兩個傳感器，它們分別以一定的頻率來測量如下數據：

• 激光雷達：測量目標車輛的座標 $(x,y)$ 。這裏的$x,y$是相對於我們的車輛的座標系的，即我們的車輛爲座標系的原點，我們的車頭爲$x$軸，車的左側爲$y$軸。
• 雷達：測量目標車輛在我們車輛座標系下與本車的距離$ρ$，目標車輛與x軸的夾角 $ψ$，以及目標車輛與我們自己的相對距離變化率 $\dot ρ$（本質上就是目標車輛的實際速度在我們和目標車輛的連線上的分量）

濾波效果：

具體的推導可以參考這篇博客：高級運動模型和擴展卡爾曼濾波

python代碼

這裏附上完整的python代碼：python_EKF_CTRV 代碼

C++代碼

這裏附上完整的C++代碼：C++_EKF_CTRV 代碼

matlab示例

下面舉一些小的例子，便於我們理解應用

拋物線demo

matlab代碼

close all;
clear all;
%%  真實軌跡模擬
kx = .01;   ky = .05;       % 阻尼係數
g = 9.8;                    % 重力
t = 15;                     % 仿真時間
Ts = 0.1;                   % 採樣週期 
len = fix(t/Ts);            % 仿真步數
dax = 3; day = 3;       % 系統噪聲
X = zeros(len,4); 
X(1,:) = [0, 50, 500, 0]; % 狀態模擬的初值
for k=2:len
    x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4); 
    x = x + vx*Ts;
    vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
    y = y + vy*Ts;
    vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
    X(k,:) = [x, vx, y, vy];
end
%%  構造量測量
dr = 8;  dafa = 0.1;        % 量測噪聲
for k=1:len
    r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
    a = atan(X(k,1)/X(k,3))*57.3 + dafa*randn(1,1);
    Z(k,:) = [r, a];
end
%% ekf 濾波
Qk = diag([0; dax/10; 0; day/10])^2;
Rk = diag([dr; dafa])^2;
Pk = 10*eye(4);
Pkk_1 = 10*eye(4);
x_hat = [0,40,400,0]';
X_est = zeros(len,4);
x_forecast = zeros(4,1);
z = zeros(4,1);
for k=1:len
    % 1 狀態預測    
    x1 = x_hat(1) + x_hat(2)*Ts;
    vx1 = x_hat(2) + (-kx*x_hat(2)^2)*Ts;
    y1 = x_hat(3) + x_hat(4)*Ts;
    vy1 = x_hat(4) + (ky*x_hat(4)^2-g)*Ts;
    x_forecast = [x1; vx1; y1; vy1];        %預測值
    % 2  觀測預測
    r = sqrt(x1*x1+y1*y1);
    alpha = atan(x1/y1)*57.3;
    y_yuce = [r,alpha]';
    %  狀態矩陣
    vx = x_forecast(2);  vy = x_forecast(4);
    F = zeros(4,4);
    F(1,1) = 1;  F(1,2) = Ts;
    F(2,2) = 1-2*kx*vx*Ts;
    F(3,3) = 1;  F(3,4) = Ts;
    F(4,4) = 1+2*ky*vy*Ts;
    Pkk_1 = F*Pk*F'+Qk;
    % 觀測矩陣
    x = x_forecast(1); y = x_forecast(3);
    H = zeros(2,4);
    r = sqrt(x^2+y^2);  xy2 = 1+(x/y)^2;
    H(1,1) = x/r;  H(1,3) = y/r;
    H(2,1) = (1/y)/xy2;  H(2,3) = (-x/y^2)/xy2;
    
    Kk = Pkk_1*H'*(H*Pkk_1*H'+Rk)^-1;       %計算增益
    x_hat = x_forecast+Kk*(Z(k,:)'-y_yuce);      %校正
    Pk = (eye(4)-Kk*H)*Pkk_1;
    X_est(k,:) = x_hat;
end
%% 
figure, hold on, grid on;
plot(X(:,1),X(:,3),'-b');
plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180));
plot(X_est(:,1),X_est(:,3), 'r');
xlabel('X'); 
ylabel('Y'); 
title('EKF simulation');
legend('real', 'measurement', 'ekf estimated');
axis([-5,230,290,530]);

上例代碼的效果圖示：

可以看出一開始收斂的不快，下面一個代碼完美解決了該問題
這裏附上一個完整的matlab代碼，效果超好。

C++示例

飛機高度demo1

假設一架飛機以恆定水平速度飛行（高度不變），地面上有一個雷達可以發射電磁波測量飛機到雷達的距離$r$。

從圖中可以看出，有如下關係：
$\theta=arctan(\frac{y}{x})\\r^2=x^2+y^2$

我們想知道某一時刻飛機的水平位置和垂直高度，以水平位置、水平速度、垂直高度作爲狀態變量：
$\mathbf{\textbf{x}}=\left(\begin{array}{c}\text{distance} \\\text{velocity} \\\text{altitude}\end{array}\right)=\left(\begin{array}{c}x \\\dot{x} \\y\end{array}\right)$
則觀測值與狀態變量之間的關係爲：$h\left(\hat{x}\right)=\sqrt{x^2+y^2}$，可以看出這是一個非線性的表達式。對於這個問題來說，觀測方程的雅克比矩陣爲：$J_H=\left[\frac{\partial h}{\partial x}\quad\frac{\partial h}{\partial \dot{x}}\quad\frac{\partial h}{\partial y}\text{ }\right]$，即
$J_H=\left[\frac{x}{\sqrt{x^2+y^2}} \quad 0 \quad \frac{y}{\sqrt{x^2+y^2}}\right]$
狀態轉移方程的雅克比矩陣爲：
$J_A=\left( \begin{array}{c} 1 & \Delta & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{array} \right)$
得到上述矩陣後我們就可以設定初值和噪聲，然後根據流程圖中的步驟進行迭代計算。

C++代碼

代碼

飛機高度demo2

飛機在2D空間中飛行，其中x軸是飛機行進的距離，y軸是飛機的高度。該系統可以由以下連續方程表示：

其中$m$是飛機的重量（1000千克）

$b_ {x}$是阻力系數（0.35 N /平方米/秒²）

$p$是提升力（3.92 N /平方米/秒²）

$g$是重力加速度（9.8米/秒²）

$u$輸入的電機的推力

計算雅各比矩陣：

即：

設置狀態向量的初始估計之後，設置代表狀態向量估計誤差的協方差的協方差矩陣P。然後，設置協方差Q和R，分別代表過程噪聲和測量噪聲的協方差矩陣。

C++代碼

代碼

EKF的不足

EKF存在的主要問題如下：

1. 運動及觀察模型用泰勒級數的一階或二階展開近似成線性模型，忽略了高階項，不可避免的引入線性誤差，甚至導致濾波器發散。有如下誤差補償方法：

   泰勒近似使得狀態預測必然存在誤差：

   A) 補償狀態預測中的誤差，附加“人爲過程噪聲”，即通過增大過程噪聲協方差來實現這一點。
   B) 擴大狀態預測協方差矩陣，用標量加權因子$φ>1$乘狀態預測協方差矩陣
   C) 利用對角矩陣$φ=diag(sqrt(φ_i)), φ_i>1$來乘以狀態預測協方差矩陣

   其實無論增大過程噪聲協方差還是狀態預測協方差矩陣，都是爲了增大kalman增益，即狀態預測是不準的，我要減小一步狀態預測在狀態更新中的權重。

2. 雅克比矩陣（一階）及海塞矩陣（二階）計算困難。二階EKF的性能要好於一階的，而二階以上的性能相比於二階並沒有太大的提高，所以超過二階以上的EKF一般不採用。但二階EKF的性能雖好，但計算量大，一般情況下不用

爲了解決這一問題，請參見下一期無損卡爾曼濾波~~

參考文獻

由淺入深的擴展卡爾曼濾波器教程