💥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
🎉3 参考文献
部分理论来源于网络，如有侵权请联系删除。
[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.
