대상 추적(1) 선형 칼만 필터(KF)
사실 표적 추적은 일종의 필터링 이론이다. 필터링에 관해서는 맨 처음 베이지안 필터링, 위너 필터링, 칼만 필터링까지입니다. 이 시리즈는 Kalman 필터링으로 시작합니다. 베이지안 필터링에 관심이 있는 어린이 신발은 B 스테이션에서 스스로 학습할 수 있습니다.
칼만 필터 원리
우리는 이산 시간 선형 확률 역학 시스템이 다음 상태 공간 모델로 설명된다고 생각합니다.
그러면 현재 상태 추정치는 다음 공식으로 표현될 수 있습니다.
그러면 예측 공분산 행렬은 다음과 같습니다.
필터링의 목적은 센서의 측정된 값을 사용하여 상태 값을 더 잘 추정할 수 있도록 하는 것입니다. 따라서 우리는 확실히 위 공식의 오차 공분산 행렬이 가능한 한 작기를 바랍니다. 즉, 위 식의 값을 최소화하기 위해서는 최적의 이득 행렬, 즉 칼만 이득을 찾아야 합니다.
Kalman gain matrix를 푸는 과정
위 식의 계산을 용이하게 하기 위해 여기서는 세 가지 정리를 제안한다.
그런 다음 P를 최소화하는 최고의 칼만 이득 행렬 K를 찾으러 갑니다.
위의 유도 후 Kalman 필터의 최적 이득 행렬을 얻었으므로 다음과 같이 Kalman 필터를 설계합니다.
Kalman 필터 설계
Kalman 필터는 실제로 위의 설명을 기반으로 합니다. 상태에 대한 사전 추정을 센서의 측정된 값과 결합하여 상태 추정을 수정하여 최종 추정(즉, 사후 추정)이 실제 상태 값에 더 가깝습니다.
위는 선형 칼만 필터의 다섯 가지 핵심 공식입니다.
마지막에 작성: 여기에서 편집자는 친구의 이해를 돕기 위해 Kalman 필터링 과정을 언어로 설명하려고 합니다.
다음 시뮬레이션 프로세스를 예로 들어 보겠습니다. 균일 모션 프로세스
먼저 Kalman 필터링을 사용해야 하는 이유를 알아내십시오.
1. 기동 과정에서 목표물은 바람 부는 것과 같은 다른 교란의 대상이 됩니다. 이러한 교란을 미리 알 수 없기 때문에 물리적 프로세스를 정확하게 설명할 수 없습니다. 즉, 상태 프로세스 방정식은 대상의 기동 프로세스를 정확하게 설명할 수 없습니다.
2. 우리 센서는 대상의 X 및 Y 위치를 측정하는 데 오류가 있어야 합니다.
위의 두 가지 사항은 Kalman 필터링을 사용하는 이유입니다.
칼만 필터링 프로세스는 어떤가요?
1. 표적의 기동 특성에 따라 표적의 다음 상태를 예측하는 물리적 모델을 설정할 수 있습니다. 즉, 프로세스 상태 방정식을 사용하여 예측하고 예측 값은 선험적 추정치가 됩니다.
2. 사전 추정이 확실히 부정확하고 오류가 있음을 알고 있으므로 이전 추정의 오류를 정량적으로 설명하는 사전 추정 공분산 행렬 P가 있습니다.
3. 우리 필터링의 목적은 이 오차를 최대한 최소화하는 것이므로 이 기준에 따라 칼만 이득 행렬의 계산식을 얻는다. 최적의 칼만 이득 행렬을 얻습니다.
4. 센서의 측정 값과 결합된 이전 추정치를 사용하여 필터링의 결과이기도 한 사후 추정치인 최상의 추정치를 얻습니다.
마지막에 쓰기
칼만 필터링의 5가지 주요 공식에서 우리는 칼만 필터링이 피드백의 성질을 가지고 있음을 알 수 있습니다. 즉, 칼만 필터링 프로세스는 수렴 프로세스입니다.
우리 삶의 많은 과정이나 측정이 비선형이기 때문에 그 효과를 잃는 것은 선형 칼만 필터입니다.비선형 과정에 대한 확장 칼만 필터와 무향 칼만 필터는 나중에 설명하겠습니다.
Matlab 시뮬레이션 분석
시뮬레이션 매개변수는 다음과 같습니다.
시뮬레이션 결과
필터링 후의 오차가 측정값의 오차보다 현저히 작은 것을 알 수 있습니다.
Matalb 코드는 다음과 같습니다.
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模型
H = [1 0 0 0;
0 0 1 0];
X0 = [100 2 200 20]';Qk = diag([0.1 0.01 0.1 0.01]);Rk = diag([1 1]);
P0 = diag([0.1 0.01 0.1 0.01]);
Xk = Track(X0,Qk,F,N/T);Zk = Measurements(Xk,Rk,H);
Xkf = EKF(X0,F,Zk,Qk,Rk,H,P0);
figure()
plot(Xk(1,:),Xk(3,:),'LineWidth',1);hold on;
plot(Xkf(1,:),Xkf(3,:),'+');hold on;
plot(Zk(1,:),Zk(2,:),'*');grid on;
xlabel('X轴/m');ylabel('Y轴/m');
legend('真实轨迹','估计轨迹','观测轨迹')
figure()
delta = sqrt((Xk(1,:)-Xkf(1,:)).^2+(Xk(3,:)-Xkf(3,:)).^2);
delta1 = sqrt((Xk(1,:)-Zk(1,:)).^2+(Xk(3,:)-Zk(2,:)).^2);
plot(delta);hold on;
plot(delta1);grid on;
legend('滤波后估计误差','直接观测的误差')
xlabel('时间/s');ylabel('距离均方误差');
%% 子函数
function Xk = Track(X0,Qk,F,N)
% 目标的真实航迹
%%输入参数
% X0 - 状态初值
% deltaX - 过程噪声标准差
% Fx - 转移矩阵
% N - 仿真总结拍
%%输出参数
% Xk - 真实航迹
% Qk - 过程噪声协方差
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,H)
% 传感器量测
%输入参数
% Xk - 真实航迹
% Rk - 量测噪声协方差
% Xstation - 观测站的坐标
%输出参数
% Zk - 传感器量测值
N = length(Xk(1,:)); % 仿真总节拍
n_z = length(H(:,1));
Zk = zeros(n_z,N);
for ii = 1:N
Zk(:,ii) = H*Xk(:,ii) + Rk*randn(n_z,1);
end
end
%--------------------------------------------------------------------------
function [Xkf] = EKF(X0,F,Zk,Qk,Rk,H,P0)
% 卡尔曼滤波
%输入参数
% X0 - 滤波初值
% F - 转移矩阵
% Zk - 传感器量测
% Qk - 过程噪声协方差
% Rk - 量测噪声协方差
%输出参数
% Xekf - 状态估计
n_x = length(X0); % 状态向量维数
N = length(Zk); % 仿真总节拍
Xkf = zeros(n_x,N);Xkf(:,1) = X0;
Pk_Last = P0; % 协方差初始化
for ii = 2:N
Xekf_hat = F*Xkf(:,ii-1); % 先验估计
Pk = F*Pk_Last*F' + Qk; % 先验协方差矩阵
Kk = Pk*H'/(H*Pk*H'+Rk); % 卡尔曼增益
Xkf(:,ii) = Xekf_hat + Kk*(Zk(:,ii)-H*Xekf_hat); % 后验估计
Pk_Last = (eye(n_x)-Kk*H)*Pk; % 更新误差协方差
end
end
}