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

原创于 2025-06-01 11:16:48 发布 · 1k 阅读

CC 4.0 BY-SA版权

文章标签：

👨‍🎓个人主页

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

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

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

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

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

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

一、卡尔曼滤波简介

卡尔曼滤波是一种用于动态系统状态估计的数学算法，尤其适用于存在噪声和不确定性的环境。1960年，卡尔曼发表了用递归方法解决离散数据线性滤波问题的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法），在这篇文章里一种克服了维纳滤波缺点的新方法被提出来，即卡尔曼滤波。

二、卡尔曼滤波的基本原理

卡尔曼滤波的基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，从而求出当前时刻的估计值。算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。

三、卡尔曼滤波的递推公式

卡尔曼滤波的递推公式包括预测阶段和更新阶段。在预测阶段，根据系统的状态方程预测下一时刻的状态和协方差矩阵。在更新阶段，利用观测值和观测方程对预测结果进行修正，得到更新后的状态估计和协方差矩阵。

四、卡尔曼滤波的变种方法

扩展卡尔曼滤波（EKF）：
- 适用于非线性系统。
- 通过对非线性系统进行线性化，使用卡尔曼滤波的框架处理非线性系统。
- 对状态转移方程和观测方程进行泰勒展开，利用一阶线性近似。
无迹卡尔曼滤波（UKF）：
- 使用一组采样点来捕获状态分布的非线性变换，而无需进行显式的线性化。
- 在处理强非线性问题时，通常表现优于EKF。

五、卡尔曼滤波的应用

卡尔曼滤波广泛应用于自动控制、信号处理、人工智能和音频工程等领域，具体包括：

导航和跟踪系统：
- 应用于飞机、船只和汽车的导航系统中。
- 在GPS等定位系统中，结合多个传感器数据提供精确的实时位置和速度估计。
机器人技术：
- 用于实时状态估计（如位置、速度、方向）。
- 结合里程计、IMU（惯性测量单元）等传感器数据，实现高精度的导航和路径规划。
金融数据分析：
- 应用于金融市场中的动态估计，如股票价格趋势的预测。
- 能够平滑市场波动，结合历史数据预测未来价格趋势。
音频信号处理：
- 用于语音信号的增强与去噪处理。
- 在音频的压缩、预测编码等场景中提高信号质量。
自动驾驶：
- 用于车辆的位置、速度和方向估计。
- 结合LIDAR、摄像头、GPS等多种传感器数据，实时更新车辆在复杂环境中的精确定位和路径规划。

六、研究趋势与展望

随着非线性系统研究的深入，卡尔曼滤波的变种方法如扩展卡尔曼滤波和无迹卡尔曼滤波等得到了广泛应用。未来，卡尔曼滤波的研究将更加注重算法的优化和实时性的提高，以适应更复杂和多变的应用场景。同时，与其他先进算法的结合也将为卡尔曼滤波的应用开辟新的领域。

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