Kalman Filter 学习笔记

最新推荐文章于 2023-07-24 03:09:47 发布

AnimateX

最新推荐文章于 2023-07-24 03:09:47 发布

阅读量1.8k

点赞数 4

分类专栏： Tracking 卡尔曼滤波 Kalman Filter

本文链接：https://blog.csdn.net/animatex/article/details/81871534

版权

Tracking 同时被 3 个专栏收录

3 篇文章 0 订阅

订阅专栏

卡尔曼滤波

1 篇文章 0 订阅

订阅专栏

Kalman Filter

1 篇文章 0 订阅

订阅专栏

Kalman Filter 学习笔记

Email: animatex.deng@gmail.com

本文目的：结合代码和理论知识，较为清楚理解基础卡尔曼滤波的原理。文章如有侵权，请联系邮箱！

卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器)，它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。

1 基础知识回顾

1.1 正太分布

$f \left( x \mid \mu, \ \sigma^2 \right)=\frac{1}{\sqrt{2\pi\sigma^2}}e^{-\frac{\left(x-\mu\right)^2}{2\sigma^2}}$

其中， $\mu$ 是分布的均值或期望; $\sigma$ 是标准差; $\sigma^2$ 是方差。

正太分布的性质：

经过线性变换高斯分布仍然为高斯分布

$X \sim N (μ, σ) Y = A X + B} \Rightarrow Y \sim N (A μ + B, A σ A T)$ $\left. {\begin{array}{*{20}{l}} {X\sim N(\mu ,\sigma )}\\ {Y = AX + B\quad \;} \end{array}} \right\}\quad \Rightarrow \quad Y\sim N(A\mu + B,A\sigma {A^T})$
高斯变量线性组合仍为高斯分布

$X 1 \sim N (μ 1, σ 21) X 2 \sim N (μ 2, σ 22)} \Rightarrow p (X 1 + X 2) \sim N (μ 1 + μ 2, σ 21 + σ 22 + 2 ρ σ 1 σ 2)$ $\left.{\begin{array}{*{20}{c}} {{X_1}\sim N({\mu _1},\sigma _1^2)}\\ {{X_2}\sim N({\mu _2},\sigma _2^2)} \end{array}} \right\} \Rightarrow p({X_1} + {X_2})\sim N\left( {{\mu _1} + {\mu _2},\sigma _1^2 + \sigma _2^2 + 2\rho {\sigma _1}{\sigma _2}} \right)$
两个相互独立的高斯变量的乘积，仍然为高斯分布

X 1 \sim N (μ 1, σ 21) X 2 \sim N (μ 2, σ 22)} \Rightarrow p (X 1) \cdot p (X 2) \sim N (σ 2 2 σ 2 1 + σ 2 2 μ 1 + σ 2 1 σ 2 1 + σ 2 2 μ 2, σ 2 1 σ 2 2 σ 2 1 + σ 2 2)

$\left. {\begin{array}{*{20}{c}} {{X_1}\sim N({\mu _1},\sigma _1^2)}\\ {{X_2}\sim N({\mu _2},\sigma _2^2)} \end{array}} \right\} \Rightarrow p({X_1}) \cdot p({X_2})\sim N\left( {\frac{{\sigma _2^2}}{{\sigma _1^2 + \sigma _2^2}}{\mu _1} + \frac{{\sigma _1^2}}{{\sigma _1^2 + \sigma _2^2}}{\mu _2},\quad \frac{{\sigma _1^2\sigma _2^2}}{{\sigma _1^2 + \sigma _2^2}}} \right)$

1.2 协方差

在概率论和统计学中，协方差是两个随机变量的联合变异性的量度。如果一个变量的较大值主要与另一个变量的较大值相对应，并且对于较小值也是如此，即变量倾向于显示相似的行为，则协方差是正的。在相反情况下，当一个变量的值越大，主要对应的更小的变量值，变量往往显示相反的行为，则协方差是负的。因此，协方差的符号显示了线性关系的趋势之间的变量。协方差的大小不容易解释，因为它不是标准化的，因此取决于变量的大小。

Covariance ( 协方差 ): 具有有限二阶矩的两个联合分布的实值随机变量 X和Y之间的协方差被定义为它们与各个预期值的偏差的预期乘积 .

Formula: $cov(X,Y)=E[(X-E[X])(Y-E(Y))]$ ， $E[X]$ 为 $X$ 的期望值。协方差有时也表示为 $\sigma_{XY}$ 或者 $\sigma(X, Y)$ 。
Simplify formula: $cov(X,Y) = E[XY]-E[X]E[Y]$ 。
性质
- $cov(X,a) = 0$
- $cov(X,X)=var(X)$
- $cov(X,Y) = cov(Y,X)$
- $cov(aX,bY)=ab \ cov(X,Y)$
- $cov(X+a, Y+b) = cov(X,Y)$
- $cov(aX+bY,cW+dV)=ac\ cov(X,W) + ad\ cov(X,V) + bc\ cov(Y,W) + bd\ cov(Y,V)$
- $cov(AX,\ BX) = A\ cov(X,\ X)\ B^T$
意义

单个随机变量的方差表示它分布的分散程度，两个随机变量的协方差就可以理解成它们一致的分散程度有多大

1.3 状态向量

状态向量：是一组数据，用于准确描述对象在空间中的位置以及对象的移动方式。从状态向量和足够的数学条件，可以确定对象的过去和未来位置。

2 Kalman Filter 简述

先看 Kalman Filter 5个标准公式：

时间更新方程

$\mathbf{X(k \mid k-1) = F_kX(k-1\mid k-1)+B_kU(k) }$
$\mathbf{P(k \mid k-1) = F_kP(k-1\mid k-1)F_k^T+Q_k}$
状态更新方程
$\mathbf{K(k) = P(k\mid k-1)H_k^T(H_kP(k\mid k-1)H_k^T +R_k)^{-1}}$
$\mathbf{X(k\mid k) = X(k\mid k-1) + K(k)(Z(k)-H_kX(k\mid k-1))}$
$\mathbf{P(k\mid k) = (1 - K(k)H_k)P(k\mid k-1)}$
参数说明

Parameter Table

Name Interpretation
$\mathbf{X(k\mid k-1)}$ 前一时刻预测当前时刻的状态
$\mathbf{X(k-1\mid k-1)}$ 前一时刻校正后的状态（最佳估计）
$\mathbf{F_k}$ 预测矩阵
$\mathbf{B_k}$ 控制矩阵
$\mathbf{U(k)}$ 控制向量（可理解为外部输入）
$\mathbf{Q_k}$ 预测过程中噪声带来的协方差矩阵
$\mathbf{P(k\mid k-1)}$ 预测值和真实值之间误差的协方差矩阵
$\mathbf{K(k)}$ 卡尔曼增益洗漱矩阵
$\mathbf{H(k)}$ 传感器数据矩阵（状态变量到测量的转换矩阵）
$\mathbf{R(k)}$ 传感器噪声协方差
$\mathbf{Z(k)}$ 传感器数据的期望矩阵

Name	Interpretation
$\mathbf{X(k\mid k-1)}$	前一时刻预测当前时刻的状态
$\mathbf{X(k-1\mid k-1)}$	前一时刻校正后的状态（最佳估计）
$\mathbf{F_k}$	预测矩阵
$\mathbf{B_k}$	控制矩阵
$\mathbf{U(k)}$	控制向量（可理解为外部输入）
$\mathbf{Q_k}$	预测过程中噪声带来的协方差矩阵
$\mathbf{P(k\mid k-1)}$	预测值和真实值之间误差的协方差矩阵
$\mathbf{K(k)}$	卡尔曼增益洗漱矩阵
$\mathbf{H(k)}$	传感器数据矩阵（状态变量到测量的转换矩阵）
$\mathbf{R(k)}$	传感器噪声协方差
$\mathbf{Z(k)}$	传感器数据的期望矩阵

2.1 Kalman Filter 框架图

首先，注意 Kalman Filter 的模型前提：

系统的状态方程是线性的；
观测方程是线性的；
过程噪声符合零均值高斯分布；
观测噪声符合零均值高斯分布；

根据前面的公式结合隐马尔科夫链 （参考数学之美第五章隐含马尔可夫模型）的知识，我们可以作出对应的框图，如下图所示：

这里写图片描述

基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一次增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

其中，我们只能看见对系统施加的作用力（对于跟踪小车而言，如：加速，刹车等）的数据 $\mathbf{U_k}$ ，以及传感器回传的数据（这里需要注意：实际拿到的数据是传感器采集数据的均值（期望）） $\mathbf{Z_k}$ 。

上述框架的流程就是：通过前一时刻状态 $\mathbf{X(k-1|k-1)}$ 结合外在因素 $\mathbf{U_k}$ 预测出当前时刻状态 $\mathbf{X(k|k-1)}$ 。再通过传感器的数据 $\mathbf{Z_k}$ 等校正当前时刻状态 $\mathbf{X(k|k-1)}$ 为 $\mathbf{X(k|k)}$ 。如此往复循环该过程。

2.2 对5个基本公式深入理解

这一节不对公式做详细的推导，仅仅进行功能上的理解。下面参考上述文章对基本的5个公式进行简单说明：

公式 1 ：通过前一时刻的最佳估计值状态和外部施加的控制来预测当前时刻状态值

$X (k ∣ k - 1) = F k X (k - 1 ∣ k - 1) + B k U (k)$ $\mathbf {X(k \mid k-1) = F_kX(k-1\mid k-1)+B_kU(k)}$
公式 2 ：结合测量噪声方差，计算预测值和真实值之间误差的协方差矩阵

$P (k ∣ k - 1) = F k P (k - 1 ∣ k - 1) F T k + Q k$ $\mathbf{P(k \mid k-1) = F_kP(k-1\mid k-1)F_k^T+Q_k}$
公式 3 ：结合公式2 和 $\mathbf{H_k}$ （状态变量到测量的转换矩阵），计算卡尔曼增益

$K (k) = P (k ∣ k - 1) H T k (H k P (k ∣ k - 1) H T k + R k) - 1$ $\mathbf{K(k) = P(k\mid k-1)H_k^T(H_kP(k\mid k-1)H_k^T +R_k)^{-1}}$
公式 4 ：结合卡尔曼增益和传感器数据以及 $\mathbf{H_k}$ 计算更新后的状态（即校正后的状态）

$X (k ∣ k) = X (k ∣ k - 1) + K (k) (Z (k) - H k X (k ∣ k - 1))$ $\mathbf{X(k\mid k) = X(k\mid k-1) + K(k)(Z(k)-H_kX(k\mid k-1))}$
公式 5 ：更新预测误差协方差矩阵

$P (k ∣ k) = (1 - K (k) H k) P (k ∣ k - 1)$ $\mathbf{P(k\mid k) = (1 - K(k)H_k)P(k\mid k-1)}$

已知一个线性系统的状态方程为：
$X k = F X K - 1 + B U k - 1 + w k - 1$ $\mathbf{X_k = F X_{K-1} + BU_{k-1} + w_{k-1}}$

其中， $\mathbf{X}$ 是系统的状态变量，其中，前一个状态是 $\mathbf{X(k-1\mid k-1)}$ ， $\mathbf{F}$ 为转换矩阵， $\mathbf{U_{k-1}}$ 为系统输入， $\mathbf{B}$ 是将输入转换为状态变量的矩阵，随机变量 $\mathbf{w_{k-1}}$ 为该线性系统的高斯噪声。注意各个矩阵的大小。这里用代码表示，如下：

% x, F, B, U, w 的矩阵大小说明
% [row, col] = size(Matrix);
    % [n, 1] = size(x);
    % [n, n] = size(F);
    % [n, k] = size(B);
    % [k, 1] = size(U);

这里用一个小车运动（匀加速），简单描述5个标准公式怎么得来。假设小车收到的外力为 $f$ ，这里用 $S$ 和 $v$ 分别表示小车运动的路程和速度。则有：

x_{t} = [\begin{matrix} S_{t} = & S_{t - 1} + v_{t - 1} \times Δ t + \frac{f_{t} (Δ t)^{2}}{2 m} \\ v_{t} = & v_{t - 1} + \frac{f_{t} Δ t}{m} \end{matrix}]

$\mathbf{x_t = \begin{bmatrix} S_t =& S_{t-1} + v_{t-1} \times \Delta{t} + \frac{f_t(\Delta{t})^2}{2m} \\ v_t =& v_{t-1} + \frac{f_t \Delta{t} }{m} \end{bmatrix}}$ ，

U t = f t m

$\mathbf{U_t=\frac{f_t}{m}}$

进一步化简：

x t = [10 Δ t 1] [S t - 1 v t - 1] + [( Δ t ) 2 2 Δ t] f t m

$\mathbf{x_t} = \begin{bmatrix} 1 & \Delta{t} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} S_{t-1} \\ v_{t-1} \end{bmatrix} + \begin{bmatrix} \frac{(\Delta t)^2}{2} \\ \Delta{t} \end{bmatrix} \frac{f_t}{m}$

即：

x t = F t x t - 1 + B t U t

$\mathbf{x_t = F_t x_{t-1} + B_t U_t}$

这里我们得到了对应的预测方程（需要注意这里的矩阵大小）。由前面可将该过程看作高斯噪声+线性马尔可夫模型，那仅靠预测方程无法实现正确的预测。那加一个（负）反馈吧，如果预测错了就通过反馈进行校正。实际上则是通过各种传感器等回传给我们数据（服从高斯分布）进行校正。

由于传感器反馈的数据不同于我们跟踪的 ‘状态’ 的单位和尺度。我们需要转换矩阵 $H_k$ 来表示传感器反馈的数据。

z k = H k x k + v k

$\mathbf{z_k = H_kx_k+{v_k}}$

其中， $\mathbf{z_k}$ 是测量值，大小是 $m \times 1$ ， $\mathbf{H_k}$ 大小是 $m\times n$ ， $\mathbf{v_k}$ 是测量噪声。

对于状态方程中的系统噪声 $\mathbf{w}$ 和测量噪声 $\mathbf{v}$ ，假设服从如下多元高斯分布，并且 $\mathbf{w，v}$ 是相互独立的。则：

p (w) \sim N (0, Q)

$p(\mathbf{w}) \sim N(0, \mathbf{Q})$

p (v) \sim N (0, R)

$p(\mathbf{v}) \sim N(0, \mathbf{R})$

其中， $\mathbf{Q，R}$ 为噪声变量的协方差矩阵。

结合前面的预测和后面的测量，我们是不是能得出实际小车的位置。则能通过当前时刻预测的的状态 $\mathbf{x(k\mid k-1)}$ 结合当前时刻的测量 $\mathbf{z(k\mid k)}$ 得到真正小车位置和速度的估计值: $\mathbf{x(k\mid k)}$

x (k ∣ k) = x (k ∣ k - 1) + K k (z k - z (k ∣ k - 1))

$\mathbf{x(k \mid k)} = \mathbf{x(k\mid k-1) }+ \mathbf{K_k(z_k - z(k\mid k-1))}$

x (k ∣ k) = x (k ∣ k - 1) + K k (z k - H x (k ∣ k - 1))

$\mathbf{x(k \mid k)} = \mathbf{x(k\mid k-1) }+ \mathbf{K_k(z_k - H x(k\mid k-1))}$

其中， $\mathbf{z(k\mid k-1)}$ 为测量值 $\mathbf{z_k}$ 的预测，则 $\mathbf{z_k - H x(k\mid k-1)}$ 越小则预测值和实际测量值越接近，即预测越准确。 $\mathbf{z_k - H x(k\mid k-1)}$ 即为测量残差。上述公式中出现的 $\mathbf{K_k}$ 如何计算？

先看误差协方差矩阵：

P (k ∣ k) = E [(x k - x (k ∣ k - 1)) (x k - x (k ∣ k - 1)) T]

$\mathbf{P(k\mid k) = E \left[ \left(x_k - x(k\mid k-1) \right) \left( x_k - x(k\mid k-1) \right)^T \right]}$

对于小车运动的模型，我们可以得到：

P (k ∣ k) = [E (S e r r S T e r r) E (v e r r S T e r r) E (S e r r v T e r r) E (v e r r v T e r r)]

$\mathbf{P(k\mid k)} = \begin{bmatrix} E(S_{err}S_{err}^T) & E(S_{err}v_{err}^T) \\ E(v_{err}S_{err}^T) & E(v_{err}v_{err}^T)\end{bmatrix}$

对其进行简化：

P (k ∣ k) = E [[(I - K k H) (x k - x (k ∣ k - 1)) - K k v k] [(I - K k H) (x k - x (k ∣ k - 1)) - K k v K] T]

$\mathbf{P(k\mid k)} = \mathbf{E\left[[(I-K_kH)(x_k - x(k \mid k-1)) - K_kv_k][(I-K_kH)(x_k - x(k\mid k-1)) - K_kv_K]^T\right]}$

P (k ∣ k) = (I - K k H) E [(x k - x (k ∣ k - 1)) (x k - x (k ∣ k - 1) T)] (I - K k H) + K K E [v k v T k] K T k

$\mathbf{P(k\mid k) = (I-K_kH)E\left[ (x_k - x(k\mid k-1))(x_k - x(k\mid k-1)^T) \right](I-K_kH)+K_KE[v_kv_k^T]K_k^T}$

即：

P (k ∣ k) = (I - K k H) P (k ∣ k - 1) (I - K k H) + K k R K T k

$\mathbf{P(k\mid k) = (I-K_kH)P(k\mid k-1)(I-K_kH) + K_kRK_k^T}$

上式对K求导，

K k = P (k ∣ k) H T (H P (k ∣ k) H T + R) - 1

$\mathbf{K_k = P(k \mid k)H^T(HP(k\mid k)H^T+R)^{-1}}$ 。再带回求出

Pk P k $\mathbf{P_k}$ 。

P (k ∣ k) = (I - K k H) P (k ∣ k - 1)

$\mathbf{P(k\mid k) = (I - K_kH)P(k\mid k-1)}$

更完整更直观的描述参考这篇文章：How a Kalman filter works 。

3 Kalman Filter 验证学习

3.1 房间温度预测示例

Kalman filter 用在一维温度数据测量系统中

function main
num_iter = 100;      % Continus mesaure 100 time.
sz = [num_iter, 1];  % Create 'vector = [100, 1]' 
x  = 26;             % The True tempture is 26
Q  = 4e-4;           % Variance of temperature Predicition
R  = 0.25;           % Measuring Variance, measurement accuracy of reaction Thermometer
T_start     = 25;    % Initial Temperature (Value) Estimate
delta_start = 1;     % Initial Estimated Variance of temperature
% The temperature initial z is the measurement result of the thermometer,
% and a Gaussian noise with a variance of 0.25 is added to the true value.
z = x + sqrt(R) * randn(sz);
state_kalman = zeros(sz);           % Initial array
% An estimate of the temperature. That is, at time k,
% combined with the current measured value of the thermometer and the predicted value at time k-1,
% the final estimated value is obtained.

variance_kalman = zeros(sz);        % Estimate of Variance
state_pre       = zeros(sz);        % Prediction of temperature
variance_pre    = zeros(sz);        % Variance of predicted values
K               = zeros(sz);        % Kalman Gain
state_kalman(1)    = T_start;       % Temperature Estimate initialization
variance_kalman(1) = delta_start;   % Estimation value Variance initialization

%% Part 2
% loop Test
for k = 2 : num_iter
    % Use the best estimate of the previous moment
    % as the prediction of the temperature at the current moment
    state_pre(k) = state_kalman(k - 1);

    % The predicted variance is the sum of the variance of the temperature optimal estimate
    % and the Gaussian noise variance at the previous moment.
    variance_pre(k) = variance_kalman(k - 1) + Q;

    % Compute Kalman Gain
    K(k) = variance_pre(k) / ( variance_pre(k) + R );

    % Combined with the measured value of the current time thermometer,
    % the prediction of the previous moment is corrected to obtain the corrected optimal estimate.
    % Since it is a direct measurement, C is 1.
    state_kalman(k) = state_pre(k) + K(k) * (z(k) - state_pre(k));

    % Calculate the variance of the final estimate for the next iteration
    variance_kalman(k) = (1 - K(k)) * variance_pre(k);
end

%% Part 3
% Plot Figure
FontSize  = 14;
LineWidth = 3;
figure( ), plot( z, 'k+' );     % Plot temperature measure Value.
hold on;
plot( state_kalman, 'b-', 'LineWidth', LineWidth )    % Plot the best estimate Value
hold on;
plot( x * ones(sz), 'g-', 'LineWidth', LineWidth );   % Plot the True Temperqture Value
legend( 'Measure Value', 'Kalman Estimate', 'True Value');
xl=xlabel( 'Time(min)' );
yl=ylabel( 'Tempture' );
set( xl, 'fontsize', FontSize );
set( yl, 'fontsize', FontSize );
hold off;
set( gca, 'FontSize', FontSize );
figure( );
valid_iter = 2 : num_iter;                            % variance_pre not valid at step 1
% Plot the variance of the optimal estimate
plot( valid_iter, variance_kalman( valid_iter ), 'LineWidth', LineWidth );
legend( 'Kalman Estimate error estimate' );
xl=xlabel( 'Time(min)' );
yl=ylabel( 'C^2' );
set( xl, 'fontsize', FontSize );
set( yl, 'fontsize', FontSize );
set( gca,'FontSize', FontSize );

3.2 根据GPS预测船的位置

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Kalman滤波在船舶GPS导航定位系统中的应用
%  原作: 黄小平
%  Revised: AnimateX
%  Date: 2018-08-20
%  Note: 船在水面（斜线运动）;
%        初始位置(-100m, 200m);
%        x方向(水平)运动速度v_x为: 2m/s
%        y方向(垂直)运动速度v_y为: 20m/s
%        GPS接收机的扫描周期T: 1s
%        观测噪声的均值为0，方差为100
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function main
clc; clear;
%% Part 1: Para Initialization
T = 1;                  % 雷达扫描周期
N = 80 / T;             % 总的采样次数
X = zeros(4, N);        % 目标的真实位置和速度， X = [S_x, v_x, S_y, v_y]^T
X(:, 1) = [-100, 2, 200, 20];         % 初始化船的位置和速度
Z = zeros(2, N);                      % 传感器对位置的预测
Z(:, 1) = [X(1, 1), X(3, 1)];         % 观测值初始化
delta_w = 1e-2;                       % 过程噪声相关参数，如果增大，曲线会明显变化。
Q = delta_w * diag([0.5, 1, 0.5, 1]); % 过程（预测状态过程）噪声方差
R = 100 * eye(2);                     % 观测噪声方差
F = [1, T, 0, 0; 0, 1, 0, 0; 0, 0, 1, T; 0, 0, 0, 1];   % 预测矩阵 F
H = [1, 0, 0, 0; 0, 0, 1, 0];                           % 观测矩阵 H
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Part 2: Kalman Filter
for t = 2 : N
    X(:, t) = F * X(:, t - 1) + sqrtm(Q) * randn(4, 1); % 目标真实轨迹
    Z(:, t) = H * X(:, t) + sqrtm(R) * randn(2, 1);     % 对目标进行观测
end
Xkf       = zeros(4, N);
Xkf(:, 1) = X(:, 1);                  % Kalman filter 滤波初始化
P0 = eye(4);                          % 协方差矩阵初始化
for i = 2 : N
    Xn = F * Xkf(:, i - 1);           % 预测
    P1 = F * P0 * F' + Q;             % 预测误差协方差
    K  = P1 * H' * inv(H * P1 * H' + R);                % 计算kalman 增益
    Xkf(:, i) = Xn + K * (Z(:, i) - H * Xn);            % 状态更新
    P0 = (eye(4) - K * H) * P1;                         % 滤波误差协方差 更新
end
for i = 1 : N
    Err_Observation(i)  = RMS(X(:, i), Z(:, i));        % 滤波前的误差
    Err_KalmanFilter(i) = RMS(X(:, i), Xkf(:, i));      % 滤波后的误差
end

%% Part 3: Plot Result
figure
hold on; box on;
plot(X(1, :), X(3, :), '-k');
plot(Z(1, :), Z(2, :), '-b.');
plot(Xkf(1, :), Xkf(3, :), '-r+');
legend('Real track', 'Observe track', 'Kalman Filter track')
figure
hold on; box on;
plot(Err_Observation,  '-ko', 'MarkerFace', 'g')
plot(Err_KalmanFilter, '-ks', 'MarkerFace', 'r')
legend('Error before filtering', 'Error after filtering')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dist = RMS(X1, X2);
if length(X2) <= 2
    dist = sqrt( (X1(1) - X2(1))^2 + (X1(3) - X2(2))^2 );
else
    dist = sqrt( (X1(1) - X2(1))^2 + (X1(3) - X2(3))^2 );
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%