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

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

---

💥1 概述

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法;卡尔曼滤波器主要根据被提取信号的测量值和预测值，通过迭代算法获得被测信号的估计值。由于迭代过程中消减了系统的量测噪声和过程噪声，因此卡尔曼滤波器可以对被测信号的精确估计，适用于解决随机信号与噪声的多维非平稳、时变、功率谱不稳定等问题[ 2]。卡尔曼滤波器包括"“预测"与"校正"两个过程;预测是利用时间更新方程建立对当前状态的先验估计，及时预估当前状态及误差协方差估计值;校正过程是利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计[~]。其具体数学方程如下:

📚2 运行结果

 

 

 

 

 

 

 

 

 

 

 

 

部分代码：

T_total = 20;       %Observation time s
T= 0.5;             %Data rate = 0.1s
N = T_total/T;
t = 0.5:T:T_total;
M = 50;              %Monto-carlo time
%Motion parameters
R0 = 80;  %km
v0 = 0.8; %km/s
v1 = -0.4; %km/s
a0 = 0;
a1 = 0.5; %km/s2
%noise
sigma_x = sqrt(0.1);     %过程噪声 / 状态噪声 的平方，此处为速度波动
sigma_z = sqrt(0.05);    %距离量测噪声的平方，高斯白

%% Kalman filter CV 1-dimension

%-------Kalman Parameters-------%
R = sigma_z^2;
P = [R   R/T     
     R/T 2*R/T^2 ];
F = [1 T 
     0 1];%状态转移矩阵
H = [1 0];%量测矩阵
%用于更新实际轨迹的转移矩阵
F_track = [1 T T^2/2
           0 1 T
           0 0 1];
%过程噪声
B = [T^2/2; T]; %过程噪声分布矩阵
v = sigma_x^2;   %x方向的过程噪声向量//相当于Q
V = B * v * B';
% %观测噪声??
% W = B * noise_x;

%------Data initial-------%
X_real = zeros(3,N);
X = zeros(2,N);
Z = zeros(1,N);
X_filter = zeros(2,N);
bias = zeros(2,N,M);
gain = zeros(2,N,M);
Cov = zeros(2,N,M);
%初始时刻1,x的位置和速度

%-------Track Initial-------%
%flag=1,Track1;flag=2,Track2;flag=3,Track3
flag = 3; 
if flag == 3
    a = a1;
else
    a = a0;
end
X_real(:,1) = [R0, v0, a]'; %x: km,km/s
X(:,1) = X_real(1:2,1);
Z(:,1) = X_real(1,1);
X_filter(:,1) = X_real(1:2,1);

%Monto-carlo

for m=1:M
    
    noise_x = randn(1,N).*sigma_x; %过程噪声
    noise_z = randn(1,N).*sigma_z; %观测噪声
    
    %构造 真实轨迹X 与 观测轨迹Z //flag = 1一次速度改变机动
    for n=2:N
        if flag == 2 && n == 16
            X_real(2,n-1) = v1;
        end
        X_real(:,n) = F_track * X_real(:,n-1);
    end
    X = X_real(1:2,:)+ B * noise_x;
    Z = H * X + noise_z;

🎉3 参考文献

部分理论来源于网络，如有侵权请联系删除。

[1]程雪聪,刘福才,黄茹楠.基于卡尔曼滤波和粒子滤波融合的UWB室内定位算法[J].计量学报,2022,43(10):1335-1340.

[2]杨佳彬,荆晶,李超,陈业明.卡尔曼滤波在试车台PLC数采系统中的应用[J].自动化技术与应用,2022,41(10):57-59.DOI:10.20033/j.1003-7241.(2022)10-0057-03.

🌈4 Matlab代码实现