👨🎓个人主页：研学社的博客
💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文目录如下：🎁🎁🎁
目录
💥1 概述
📚2 运行结果
🎉3 参考文献
🌈4 Matlab代码实现
Kalman滤波算法需以系统的时间离散化状态空间为基础",测量过程的计算方程为：
部分代码：
%状态转移矩阵
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
部分理论来源于网络，如有侵权请联系删除。
[1]代云锋.自适应卡尔曼滤波在标准贯入度动态观测数据处理中的应用[J].测绘与空间地理信息,2022,45(08):184-188+192.
[2]蒋锐,李俊,徐友云,王小明,李大鹏.基于联邦卡尔曼滤波器的容错GPS-AOA-SINS组合导航算法[J].通信学报,2022,43(08):78-89.
[3]闫辉,周国华.基于Kalman滤波的船舶磁化干扰系数测量算法[J].中国舰船研究,2022,17(04):164-169.DOI:10.19693/j.issn.1673-3185.02273.
