【信号处理】卡尔曼（Kalman）滤波（Matlab代码实现）

news2024/11/22 19:54:15

👨‍🎓个人主页：研学社的博客

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

Kalman滤波算法需以系统的时间离散化状态空间为基础",测量过程的计算方程为：

📚2 运行结果

部分代码：

%状态转移矩阵
F = [1 T 0 0
0 1 0 0
0 0 1 T
0 0 0 1];
H = [1 0 0 0
0 0 1 0];
%过程噪声
B = [T^2/2, 0; T, 0;
0, T^2/2; 0, T]; %过程噪声分布矩阵
v = sigma_u^2; %x方向的过程噪声向量//相当于Q
V = B * v * B';
% %观测噪声??
% W = B * noise_x;

%------Data initial-------%
X_real = zeros(4,N);
X = zeros(4,N);

Z1 = zeros(2,N);
X_EKF1 = zeros(4,N);
% P1 = zeros(4,4,N);
% K1 = zeros(4,2,N);
% Hj1 = zeros(2,4,N);
Z2 = zeros(2,N);
Z_polar2 = zeros(2,N);
X_EKF2 = zeros(4,N);
% P2 = zeros(4,4,N);
% K2 = zeros(4,2,N);
% Hj2 = zeros(2,4,N);

X_CC = zeros(4,N);
X_BC = zeros(4,N);
bias = zeros(8,N,M);

%-------Track Initial-------%
X_real(:,1) = [Rx, vx, Ry, vy]'; %x: km,km/s

X_EKF1(:,1) = [Rx, 0, Ry, 0];
X_EKF2(:,1) = [Rx, 0, Ry, 0];
X_CC(:,1) = [Rx, 0, Ry, 0];
X_BC(:,1) = [Rx, 0, Ry, 0];

%Monto-carlo
for m=1:M

noise_x = randn(2,N).*sigma_x; %过程噪声
noise_z1 = randn(2,N).*sigma_z; %观测噪声
noise_z2 = randn(2,N).*sigma_z;

%构造真实轨迹X 与观测轨迹Z
for n=2:N
if n == 30
X_real(2,n-1) = 1;
end
X_real(:,n) = F * X_real(:,n-1);
end
X = X_real + B * noise_x;
Z1= H * X + noise_z1 - [x1,0;0,y1]*ones(2,N);
Z2= H * X + noise_z1 - [x2,0;0,y2]*ones(2,N);

%这里可以写成function的形式
P_BC = P1;
for n=2:N
x_predict = F * X_EKF1(:,n-1); %状态一步预测
p_predict = F * P1 * F'+ V; %协方差一步预测
S = H * p_predict * H'+ R1; %新息协方差
K1 = p_predict * H'/ S ; %增益
X_EKF1(:,n) = x_predict + K1 * (Z1(:,n) - H * x_predict + [x1;y1]); %状态更新方程
P1 = (eye(4)-K1*H) * p_predict; %协方差更新方程 %后面一半要不要？

x_predict2 = F * X_EKF2(:,n-1); %状态一步预测
p_predict2 = F * P2 * F'+ V; %协方差一步预测
S2 = H * p_predict2 * H'+ R2; %新息协方差
K2 = p_predict2 * H'/ S2 ; %增益
X_EKF2(:,n) = x_predict2 + K2 * (Z2(:,n) - H * x_predict2 + [x2;y2]); %状态更新方程
P2 = (eye(4)-K2*H) * p_predict2; %协方差更新方程 %后面一半要不要？

P_CC = inv( inv(P1) + inv(P2));
X_CC(:,n) = P_CC * (P1\X_EKF1(:,n) + P2\X_EKF2(:,n));

P_BC = (eye(4)-K2*H)* F*P_BC*F'*(eye(4)-K1*H)';
X_BC(:,n) = X_EKF2(:,n)+(P2-P_BC)/(P1+P2-2*P_BC)*(X_EKF1(:,n)-X_EKF2(:,n));
end