Target Tracking (2): Extended Kalman Filter (EKF)

We have already described the linear Kalman filter (KF) in the previous article, and also carried out a detailed theoretical derivation. However, since there are many nonlinear links in our life, this makes the linear Kalman filter unusable. In this article, we will discuss and introduce the Extended Kalman Filter (EKF) for nonlinear links. Of course, the prerequisite for understanding this article is to understand the previous article. If the previous article has been understood, then this article is very simple.

write on top

Here, I wonder if some friends have the following questions?

Why can't linear Kalman filter be used when it becomes a nonlinear link?
When it becomes a nonlinear link, the superposition theorem and the homogeneity theorem are no longer satisfied. The purpose of our filtering is to remove the noise. If we assume that the noise is subject to a Gaussian distribution, when passing through the nonlinear link, the distribution of the noise must no longer be a Gaussian distribution, so we cannot use linear Kalman filtering before recursion.

Therefore we introduce nonlinear filtering

Principle of Extended Kalman Filter (EKF)

insert image description here

The above is the core content of the expanded Kalman filter.

write at the end

Here, the editor wants to say that in fact, the extended Kalman filter is to linearize the nonlinear link, use Taylor expansion, only use the first order, and omit the higher order items. When we do this, we introduce linearization error again, because we artificially omit higher-order terms. In the next article, we will introduce another linearization method, namely unscented transform and unscented Kalman filter (UKF).

Matlab simulation analysis

Simulation parameters

insert image description here
Here, we only set the observation link as a nonlinear link, and the observation quantity is the angle of the target relative to the observation station.

insert image description here

According to the simulation results, we can see that when the observation link changes from a linear link to a nonlinear link, the tracking error increases significantly.

The Matalb code is as follows

% 拓展卡尔曼滤波在目标跟踪过程中的应用，非线性环节为观测环节，观测的目标的方位
% 观测方程：Z(k) = atan((y(k)-Xstation(2))/(x(k)-Xstation(1))) + v(k)
clear;close all;
T = 1;
N = 60;
F = [1,T,0,0;
     0,1,0,0;
     0,0,1,T;
     0,0,0,1]; %二维CV模型
X0 = [100 2 200 20]';Qk = diag([0.1 0.01 0.1 0.01]);Rk = 0.1;
P0 = diag([0.1 0.01 0.1 0.01]);
Xstation = [0;0];
Xk = Track(X0,Qk,F,N/T);Zk = Measurements(Xk,Rk,Xstation); % 观测方程为非线性表达，所以直接在函数里面进行计算
[Xekf] = EKF(X0,F,Zk,Qk,Rk,P0,Xstation);

figure()
plot(Xk(1,:),Xk(3,:),'LineWidth',1);hold on;
plot(Xekf(1,:),Xekf(3,:),'LineWidth',1);grid on;
legend('真实轨迹','EKF估计航迹')
delta = sqrt((Xk(1,:)-Xekf(1,:)).^2+(Xk(3,:)-Xekf(3,:)).^2);
figure()
plot(delta);grid on
xlabel('时间/s');ylabel('距离均方误差');
legend('拓展Kalman滤波均方误差')

%--------------------------------------------------------------------------
%% 子函数部分
function Xk = Track(X0,Qk,F,N)
% 目标的真实航迹
%%输入参数
% X0 - 状态初值
% Qk - 过程噪声协方差
% Fx - 转移矩阵
% N - 仿真总结拍
%%输出参数
% Xk - 真实航迹

n_x = length(X0); % 状态向量维度
Xk = zeros(n_x,N);Xk(:,1) = X0;
for ii = 2:N
    Xk(:,ii) = F*Xk(:,ii-1) + Qk*randn(n_x,1);

end
end
%--------------------------------------------------------------------------
function Zk = Measurements(Xk,Rk,Xstation)
% 传感器量测
%输入参数
% Xk - 真实航迹
% Rk - 量测噪声协方差
% Xstation - 观测站的坐标
%输出参数
% Zk - 传感器量测值


N = length(Xk(1,:)); % 仿真总节拍
Zk = zeros(1,N);
for ii = 1:N
    Zk(ii) = atan((Xk(3,ii)-Xstation(2))/(Xk(1,ii)-Xstation(1)))+Rk*randn(1);
end
end
%--------------------------------------------------------------------------
function [Xekf] = EKF(X0,F,Zk,Qk,Rk,P0,Xstation)
% 拓展卡尔曼滤波
%输入参数
% X0 - 滤波初值
% F - 转移矩阵
% Zk - 传感器量测
% Qk - 过程噪声协方差
% Rk - 量测噪声协方差
% Xstation - 观测站坐标
%输出参数
% Xekf - 状态估计

n_x = length(X0); % 状态向量维数
N = length(Zk); % 仿真总节拍
Xekf = zeros(n_x,N);Xekf(:,1) = X0;
Pk_Last = P0; % 协方差初始化
for ii = 2:N
    Xekf_hat = F*Xekf(:,ii-1); % 先验估计
    Pk = F*Pk_Last*F' + Qk; % 先验协方差矩阵
    H = [-(Xekf_hat(3)-Xstation(2))/((Xekf_hat(1)-Xstation(1))^2+(Xekf_hat(3)-Xstation(2))^2),0,...
        (Xekf_hat(1)-Xstation(1))/((Xekf_hat(1)-Xstation(1))^2+(Xekf_hat(3)-Xstation(2))^2),0];%即为所求一阶近似
    Kk = Pk*H'/(H*Pk*H'+Rk); % 卡尔曼增益
    Zzk = atan((Xekf_hat(3)-Xstation(2))/(Xekf_hat(1)-Xstation(1)));
    Xekf(:,ii) = Xekf_hat + Kk*(Zk(ii)-Zzk); % 后验估计
    Pk_Last = (eye(n_x)-Kk*H)*Pk; % 更新误差协方差
end
end