卡尔曼滤波算法-Matlab仿真

1 实验要求
设有一雷达对某飞行器进行观测，飞行器状态参数为径向距离、速度、加速度。假设飞行器相对雷达径向作匀加速直线运动，并忽略加速度扰动。有初始条件如下图。
观测信号为r， 2秒观测一次，观测噪声为零均值白噪声，方差为0.15(km)^2。
分析该运动情况，并使用Kalman滤波器对观测过程进行仿真。
2 原理
1）卡尔曼滤波算法
卡尔曼估计可分为两个过程：预测和校正。
预测也称时间更新，是使用上一状态的卡尔曼估计值，对当前状态做出预测。
校正也称滤波更新，是使用当前状态的观测值，修正预测值，以获得当前状态的卡尔曼估计值。
2）建模分析
实验所描述的场景可视为离散时间线性随机动态系统，假定所有随机变量都是Gauss的，且状态更新、过程噪声、测量噪声这三个随机过程彼此独立，则对于任意损失函数可以使用基本Kalman滤波公式进行求解。
3 仿真代码

clear
%%
% -------------------------------1.初始化-----------------------------------
Total_time = 200;  % 观测总时长
Interval_time = 2;  % 观测间隔
N = Total_time/Interval_time;  % 观测点数X_init = [0 0 0.2]';  % 初始状态值
P_kalm = diag([8 10 5]);  % 初始误差值
V = 0.15;  % 观测噪声F = [1 2 2;0 1 2;0 0 1];  % 状态转移矩阵-F
H =[1 0 0];  % 观测矩阵-H%%% ----------------------------2.定义3条轨迹--------------------------------X_true_traj = zeros(3,N);  % 真实轨迹X_true_traj(:,1) = X_init;X_true_traj(3,:) = 0.2;Z_meas_traj = zeros(1,N);  % 观测轨迹Z_meas_traj(:,1) = 0;X_pred_traj = zeros(3,N);  % 预测轨迹X_pred_traj(:,1) = X_init;X_kalm_traj = zeros(3,N);  % 滤波轨迹X_kalm_traj(:,1) = X_init;%% % ----------------------3.离散状态方程和观测方程的迭代----------------------
for i = 2 : NX_true_traj(:,i) = F * X_true_traj(:,i-1);  % 状态方程Z_meas_traj(i) = H * X_true_traj(:,i) + sqrt(V) * randn;  % 观测方程
end%%% -------------------------4.进行卡尔曼滤波的迭代--------------------------
for i = 2 : N  % 每两秒进行一次观测%时间更新X_pred_traj(:,i) = F * X_kalm_traj(:,i-1);  % 状态预测值P_pred = F * P_kalm * F';  % 预测值方差%量测更新值  K = P_pred * H' * inv(H * P_pred * H'+ V);  % 卡尔曼增益X_kalm_traj(:,i) = X_pred_traj(:,i) + K * (Z_meas_traj(:,i) - H * X_pred_traj(:,i));  % 状态滤波值 P_kalm = (eye(3)-K * H) * P_pred;  % 滤波值方差
end%%
% ------------------------------5.绘制轨迹---------------------------------
% 距离变化轨迹图
figure(1);
plot((1:N),X_true_traj(1,:),'-black',(1:N),Z_meas_traj(1,:),'ob',(1:N),X_pred_traj(1,:),'+r',(1:N),X_kalm_traj(1,:),'xg')
title('距离变化轨迹');
xlabel('时间t/s');
ylabel('距离r/km');
legend('真实轨迹','观测轨迹','预测轨迹','滤波轨迹');
hold on;% 速度变化轨迹
figure(2);
plot((1:N),X_true_traj(2,:),'-black',(1:N),X_pred_traj(2,:),'+r',(1:N),X_kalm_traj(2,:),'xg')
title('速度变化轨迹');
xlabel('时间t/s');
ylabel('速度v/(km/s)');
legend('真实轨迹','预测轨迹','滤波轨迹');
hold on;% 误差变化轨迹图
figure(3)
plot((1:N),abs(X_true_traj(1,:)-X_kalm_traj(1,:)),'b',(1:N),abs(X_true_traj(1,:)-Z_meas_traj(1,:)),'r');
title('误差变化轨迹')
xlabel('时间t/s')
ylabel('误差值/km ')
ylim([-10 10]);
legend('滤波值误差', '观测值误差');
hold off;

卡尔曼滤波算法-Matlab仿真相关推荐

ICP算法MATLAB仿真
2022.4.15再次附上数据satellite.txt新链接: 链接:https://pan.baidu.com/s/1mEN-FQbTlHOWfxCHyu0Dxg 提取码:02i4 (永久有效) ...
Matlab算法DSP移植验证,DSP计算机作业自适应噪声抵消LMS算法Matlab仿真
[实例简介] 自适应噪声抵消LMS算法Matlab仿真,DSP计算机作业数字信号处理自适应 1) 借助MATLAB画出误差性能曲面和误差性能曲面的等值曲线: 2) 写出最陡下降法, LMS算法的计 ...
【ISAR成像定标方法（1）—转台目标的RD成像算法MATLAB仿真】
目录前提介绍 ISAR成像基本流程 ISAR成像转台模型与回波建模转台模型发射信号与回波信号分析转台目标RD成像算法转台目标RD成像的仿真实验与分析结语前提介绍 ISAR成像定标方法的 ...
用matlab仿真导航信号,北斗卫星导航信号串行捕获算法MATLAB仿真报告(附MATLAB程序)[互联网+]...
<北斗卫星导航信号串行捕获算法MATLAB仿真报告(附MATLAB程序)[互联网+]>由会员分享,可在线阅读,更多相关<北斗卫星导航信号串行捕获算法MATLAB仿真报告(附MATLA ...
用matlab仿真导航信号,北斗卫星导航信号串行捕获算法MATLAB仿真报告(附MATLAB程序)...
<北斗卫星导航信号串行捕获算法MATLAB仿真报告(附MATLAB程序)>由会员分享,可在线阅读,更多相关<北斗卫星导航信号串行捕获算法MATLAB仿真报告(附MATLAB程序)(9 ...
卫星测控matlab程序,北斗卫星导航信号串行捕获算法MATLAB仿真报告(附MATLAB程序).docx...
PAGE 4 北斗卫星导航信号串行捕获算法MATLAB仿真报告原理卫星导航信号的串行捕获算法如图1所示. 图1 卫星导航信号的串行捕获算法接收机始终在本地不停地产生对应某特定卫星的本地伪码,并且 ...
一个简单的基于形态学处理的报纸图像版面分割算法matlab仿真
目录一.理论基础二.部分MATLAB仿真三.仿真结论分析一.理论基础图像分割就是把图像分成若干个特定的.具有独特性质的区域并提出感兴趣目标的技术和过程.它是由图像处理到图像分析的关键步骤.现 ...
条形碎片图纸的自动拼接规则算法matlab仿真
目录 1.算法概述 2.仿真效果 3.matlab仿真源码 1.算法概述破碎文件的拼接在司法物证复原.历史文献修复以及军事情报获取等领域都有着重要的应用.目前发现对碎纸片的拼接大部分由人工完成,准确 ...
基于Gabor滤波器的指纹图像增强处理算法matlab仿真
目录 1.算法概述 2.仿真效果 3.MATLAB仿真源码 1.算法概述 Gabor 变换是一种加窗短时 Fourier 变换(简单理解起来就是在特定时间窗内做 Fourier 变换),是短时 Fou ...
基于Frangi滤波的视网膜静脉血管检测算法matlab仿真
目录 1.算法概述 2.仿真效果 3.MATLAB仿真源码 1.算法概述随着图像数字化处理的快速发展,医学图像处理越来越受到人们的广泛关注.研究表明,人体许多全身性疾病都与眼底血管的异常有着密切的联 ...

卡尔曼滤波算法-Matlab仿真

卡尔曼滤波算法-Matlab仿真

卡尔曼滤波算法-Matlab仿真相关推荐

最新文章

热门文章