Zielverfolgung (1) Linearer Kalman-Filter (KF)
Tatsächlich ist die Zielverfolgung eine Art Filtertheorie. Wenn es um Filterung geht, von der Bayes'schen Filterung über die Wiener-Filterung bis hin zur Kalman-Filterung. Diese Serie beginnt mit der Kalman-Filterung. Kinderschuhe, die sich für Bayes'sche Filterung interessieren, können an der B-Station selbst lernen.
Prinzip des Kalman-Filters
Wir gehen davon aus, dass das zeitdiskrete lineare stochastische dynamische System durch das folgende Zustandsraummodell beschrieben wird:
Dann kann die aktuelle Zustandsschätzung durch die folgende Formel ausgedrückt werden:
Dann ist die vorhergesagte Kovarianzmatrix:
Der Zweck unserer Filterung besteht darin, den Messwert des Sensors in die Lage zu versetzen, unseren Zustandswert besser abzuschätzen. Daher hoffen wir natürlich, dass die Fehlerkovarianzmatrix der obigen Formel so klein wie möglich ist. Das heißt, wir müssen eine optimale Verstärkungsmatrix finden, nämlich die Kalman-Verstärkung, um den Wert der obigen Formel zu minimieren.
Der Prozess der Lösung der Kalman-Gain-Matrix (der Herausgeber hat direkt einen Screenshot des organisierten Dokuments gemacht)
Um die Berechnung der obigen Formel zu erleichtern, werden hier drei Theoreme vorgeschlagen.
Dann suchen wir die beste Kalman-Verstärkungsmatrix K, die P minimiert
Nach der obigen Ableitung haben wir die optimale Verstärkungsmatrix des Kalman-Filters erhalten und werden dann den Kalman-Filter unten entwerfen.
Design des Kalman-Filters
Der Kalman-Filter basiert tatsächlich auf der obigen Beschreibung und verwendet unsere vorherige Schätzung des Zustands in Kombination mit dem gemessenen Wert des Sensors, um unsere Schätzung des Zustands zu korrigieren, sodass unsere endgültige Schätzung (dh die hintere Schätzung) möglich ist näher am tatsächlichen Zustandswert sein.
Das Obige sind die fünf Kernformeln des linearen Kalman-Filters.
Am Ende geschrieben: Hier möchte der Herausgeber den Prozess der Kalman-Filterung im Volksmund erklären, um Freunden das Verständnis zu erleichtern
Nehmen wir als Beispiel den folgenden Simulationsprozess, den gleichförmigen Bewegungsvorgang
Finden Sie zunächst heraus, warum wir die Kalman-Filterung verwenden müssen.
1. Während des Manövriervorgangs ist das Ziel anderen Störungen ausgesetzt, beispielsweise Wind. Da wir diese Störungen nicht im Voraus kennen können, können wir den physikalischen Prozess nicht genau beschreiben, das heißt, unsere Zustandsprozessgleichung kann den Manövrierprozess des Ziels nicht genau beschreiben.
2. Unser Sensor muss Fehler bei der Messung der X- und Y-Positionen des Ziels aufweisen.
Die beiden oben genannten Punkte sind der Grund, warum wir die Kalman-Filterung verwenden.
Wie sieht der Kalman-Filterungsprozess aus?
1. Basierend auf den Manövriereigenschaften des Ziels können wir ein physikalisches Modell erstellen, um den nächsten Zustand des Ziels vorherzusagen. Das heißt, wir verwenden unsere Prozesszustandsgleichung, um Vorhersagen zu treffen, und der vorhergesagte Wert wird zu einer A-priori-Schätzung.
2. Wir wissen, dass unsere vorherige Schätzung definitiv ungenau ist und Fehler vorliegen, daher haben wir eine apriorische Schätzungskovarianzmatrix P, die den Fehler unserer vorherigen Schätzung quantitativ beschreibt.
3. Der Zweck unserer Filterung besteht darin, diesen Fehler so weit wie möglich zu minimieren, sodass gemäß diesem Kriterium die Berechnungsformel der Kalman-Verstärkungsmatrix erhalten wird. um unsere optimale Kalman-Verstärkungsmatrix zu erhalten.
4. Wir verwenden unsere vorherige Schätzung in Kombination mit dem Messwert des Sensors, um unsere beste Schätzung zu erhalten, d. h. die hintere Schätzung, die auch das Ergebnis unserer Filterung ist.
schreibe am Ende
Aus den fünf Hauptformeln der Kalman-Filterung können wir ersehen, dass die Kalman-Filterung den Charakter einer Rückkopplung hat, das heißt, der Kalman-Filterungsprozess ist ein konvergenter Prozess.
Da viele Prozesse oder Messungen in unserem Leben nichtlinear sind, verliert der lineare Kalman-Filter seine Wirkung. Wir werden später den erweiterten Kalman-Filter und den unscented Kalman-Filter für nichtlineare Prozesse beschreiben.
Matlab-Simulationsanalyse
Die Simulationsparameter sind wie folgt
Simulationsergebnisse
Wir sehen, dass der Fehler nach der Filterung deutlich kleiner ist als der Fehler des Messwertes.
Der Matalb-Code lautet wie folgt:
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
}