使用matlab对输入数据进行卡尔曼滤波

使用matlab对输入数据进行卡尔曼滤波。

简单说明一下程序中的数据。假设一辆汽车从初始点（0，10）开始行驶，初始速度沿y轴正方向10m/s。然后在观测途中向右先加速再减速变换车道。

整个过程其实有x轴坐标，y轴坐标，x轴速度，y轴速度以及x轴的加速度这5个物理量。但在卡尔曼滤波时我只用了x轴坐标，y轴坐标，x轴速度，y轴速度这4个量，相当于将x轴的人为施加的加速度当作了噪声处理。人为施加的加速度当然不符合高斯分布，有违卡尔曼滤波的假设，这里只是为现实应用直接处理的。有兴趣的可以把x轴的加速度这个量也纳入到卡尔曼滤波中。

clc
clear
close all
%% 汽车变道数据生成
% 汽车初始位置为（0，10）,初始x方向速度为0，y方向速度为5m/s，向前匀速运动
% 在第400-600帧时，向右加速，加速度为0.05m/s^2
% 在第601-610帧时，停止加速
% 在第611-810帧时，向左加速，加速度为0.05m/s^2，正好使得x轴方向速度为0，完成变道%间隔时间步长.每秒30帧
T = 1/30;
%总的观测帧数目
totaltime = 1600;
% 状态转移矩阵F
f_1 = [1 T T^2/2;0 1 T;0 0 1];
f_2 = [1 T;0 1];
F = blkdiag(f_1,f_2);
% [初始x坐标， 初始x速度， 初始x加速度， 初始y坐标， 初始y速度]
X(:,1) = [0 0 0 10 5]';
% 观测矩阵H，观测到x坐标，x速度， y坐标， y速度四个量
H = [1 0 0 0 0;0 1 0 0 0;0 0 0 1 0; 0 0 0 0 1];
% 过程噪声，可能路面比较滑，风力等影响，对下个时刻的真实状态造成影响
sigma = 0.00005;
Q = diag([sigma sigma/T 0 sigma sigma/T]);
% 传感器的观测噪声矩阵R
R = diag([0.25, 0.25, 0.25, 0.25]);
% 观测到的数据
Z(:,1) = H * X(:,1) + sqrtm(R) * randn(4,1);  for i = 2 : totaltime% 当前时刻的状态由上一时刻的状态变换过来，加上过程噪声QX(:,i) = F * X(:, i-1) + Q * randn(5,1);% 司机手动输入导致加速度改变% 400-600 车辆向右加速变道if i>400 && i<=600X(3,i) = 0.05;end% 600-610 车辆匀速变道if i>600 && i<=610X(3,i) = 0;end% 610-810 车辆减速变道if i>610 && i<=810X(3,i) = -0.05;end% 810以后 变道完成if i>810X(3,i) = 0;end% 测量结果由当前时刻的真实数据经过测量矩阵H，然后加上观测噪声矩阵R得到Z(:,i) = H * X(:, i) + R * randn(4,1);
end%% 卡尔曼滤波
% 卡尔曼滤波后的目标结果[x坐标， x速度， y坐标， y速度]
Xkf(:,1) = [X(1:2,1); X(4:5,1)];
Xkf(:,1) = [10; 2; 15; 8];
f_1 = [1 T;0 1];
f_2 = [1 T;0 1];
F = blkdiag(f_1,f_2);
sigma = 0.0005;
% Q表示过程噪声。由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。
% 工程上，我们一般将Q设置为单位矩阵参与运算
Q = diag([sigma sigma sigma sigma]);
H = eye(4);
% Xkf(:,1) = [3; 0; 0; 20; 5];
P0 = 100*eye(4);
for i = 2: totaltime% 卡尔曼滤波7公式（y和S是中间变量，简化后是卡尔曼滤波5公式）% 根据上一时刻的状态预测这一时刻的状态Xn = F * Xkf(:,i-1);% P矩阵表示系统的不确定程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，% 随着越来越多的数据注入滤波器中，不确定程度会变小，P的专业术语叫状态协方差矩阵P1 = F * P0 * F' + Q;% y代表实际观测到的测量值Z(:,i)与预测值H * Xn之间差值y = Z(:,i) - H * Xn;% 卡尔曼增益K,预测的状态与当前时刻的测量值进行加权,就是y的权值(S是临时变量)S = H * P1 * H' + R;K = P1 * H' * inv(S);% 完成对当前状态向量x的更新,不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声Xkf(:,i) = Xn + K * y;% 根据卡尔曼增益，更新系统的不确定度P，用于下一个周期的运算P0 = (eye(4) - K * H) * P1;
end%% 最终结果
figure
hold on;
box on;
grid on
plot(Z(1,:),Z(3,:),'b.');
plot(X(1,:),X(4,:),'-r');
plot(Xkf(1,:),Xkf(3,:),'-k');
legend('观测点','真实轨迹','滤波轨迹')
xlabel('X');
ylabel('Y');
daspect([0.03 1 1]);figure
hold on;
box on;
grid on
plot(Z(2,:),Z(4,:),'-b');
plot(Xkf(2,:),Xkf(4,:),'-k');
plot(X(2,:),X(5,:),'-r');
legend('观测速度','滤波速度','真实速度')
xlabel('X方向速度');
ylabel('Y方向速度');
daspect([1 1 1]);

最终结果：

参考资料：卡尔曼滤波（一）和线性卡尔曼滤波算法及示例

使用matlab对输入数据进行卡尔曼滤波相关推荐

【语音去噪】基于matlab谱减法+维纳滤波+卡尔曼滤波语音去噪【含Matlab源码 1881期】
⛄一.谱减法+维纳滤波+卡尔曼滤波语音去噪简介 1 维纳滤波算法在传统的去噪算法中,维纳滤波因其操作简单.去噪效果好,被公认为一种经典的去噪算法.语音信号在时域的表示为: yi( t) = si( ...
牛客网matlab怎么输入数据,从牛客上找的面经
转自:牛客网作者:雲水謡链接:https://www.nowcoder.com/discuss/262804 来源:牛客网 OPPO-计算机视觉算法(sp) 一面: 大约40分钟吧,主要聊项目,问 ...
新息自适应卡尔曼滤波matlab代码,基于自适应卡尔曼滤波的弱信号跟踪方法与流程...
本发明涉及卫星导航接收机中在微弱信号下的跟踪技术,提供了一种基于自适应卡尔曼滤波的弱信号跟踪方法. 背景技术: 随着众多新的导航卫星系统的投入和运行,全球导航卫星系统(globalnavigation ...
基于扩展卡尔曼滤波的SOC估计（附MATLAB代码）
1.卡尔曼滤波原理原理可以参考我之前学习的笔记,使用goodnote完成的. 我认为,对于公式的推导不需要做太多深入的了解,我之前也对公式进行推导的理解,但是没过几天就忘了,只需要掌握住那重要的5个 ...
卡尔曼滤波（Kalman Filtering）——（6）MATLAB仿真（保姆级）
MATLAB仿真一.卡尔曼滤波的实际应用二.流程图三.执行过程四.程序代码五.仿真结果参考文献一.卡尔曼滤波的实际应用在这里依旧以前面提到的测量硬币为例进行MATLAB仿真.现有一 ...
MATLAB 基础与通信系统仿真
文章目录第 1 章 MATLAB 基础与通信系统仿真 1.1 MATLAB 简介 1.1.1 MATLAB 的起源 1.1.2MATLAB 的特点 1.2 MATLAB 程序设计 1.2.1 MAT ...
小波相干wtc matlab,WaveletSpectra 小波相干分析代码,图谱专用，用于做性 matlab 238万源代码下载- www.pudn.com...
文件名称: WaveletSpectra下载收藏√ [ 5 4 3 2 1 ] 开发工具: matlab 文件大小: 4165 KB 上传时间: 2014-03-25 下载次数: 5 提 ...
使用matlab建立Kd-tree并进行k-NN查询
文章中关于搜索算法的实现部分貌似有些问题,后面有一篇文章重新写了一遍关于kd-tree的内容,欢迎参阅,这篇文章的错误目前没有修正. 使用matlab对输入数据建立Kd-tree并通过Kd-tree进 ...
基于EKF的四旋翼无人机姿态估计matlab仿真
目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB 1.算法描述卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器),它能够从一系列的不完全包含噪声的测量中,估计动态 ...

使用matlab对输入数据进行卡尔曼滤波

使用matlab对输入数据进行卡尔曼滤波相关推荐

最新文章

热门文章