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

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

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

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

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

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

目录

💥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

🎉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.

🌈4 Matlab代码实现