ターゲット追跡 (2): 拡張カルマンフィルター (EKF)

前回の記事で線形カルマンフィルター (KF) について説明し、詳細な理論的導出も実行しました。しかし、私たちの生活には多くの非線形リンクがあるため、線形カルマンフィルターは使用できなくなります。この記事では、非線形リンク用の拡張カルマンフィルター (EKF) について説明し、紹介します。もちろん、この記事を理解するには前回の記事を理解していることが前提となります。前の記事を理解していれば、この記事は非常に簡単です。

上に書く

ここで、友人の中には次のような疑問を持つ人もいるのではないだろうか？

非線形リンクになると線形カルマンフィルターが使用できないのはなぜですか?
非線形リンクになると重ね合わせ定理や一様性定理が満たされなくなります。フィルタリングの目的はノイズを除去することです。ノイズがガウス分布に従うと仮定すると、非線形リンクを通過するとき、ノイズの分布はガウス分布ではなくなるため、線形カルマンを使用することはできません。再帰前のフィルタリング。

そこで、非線形フィルタリングを導入します。

拡張カルマンフィルター (EKF) の原理

ここに画像の説明を挿入

上記は、拡張カルマンフィルターの中心的な内容です。

最後に書きます

ここで、編集者は、実際には、拡張カルマンフィルターは非線形リンクを線形化し、テイラー展開を使用し、一次のみを使用し、高次の項目を省略するものであると言いたいのです。これを行うと、高次項を人為的に省略するため、線形化誤差が再び発生します。次の記事では、別の線形化手法、つまりアンセンテッド変換とアンセンテッドカルマンフィルター (UKF) を紹介します。

Matlab シミュレーション解析

シミュレーションパラメータ

ここに画像の説明を挿入
ここでは、観測リンクを非線形リンクとして設定するだけであり、観測量は観測点に対するターゲットの角度です。

ここに画像の説明を挿入

シミュレーション結果によると、観測リンクが線形リンクから非線形リンクに変化すると、追跡誤差が大幅に増加することがわかります。

Matalbのコードは次のとおりです

% 拓展卡尔曼滤波在目标跟踪过程中的应用，非线性环节为观测环节，观测的目标的方位
% 观测方程：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