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

news2024/12/25 9:27:23

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

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

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

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

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

目录

💥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;