经典卡尔曼滤波器直接调用代码(Matlab)

clc,clear;
%以下开始进行交互式输入等操作
paraZkstr = inputdlg('请输入观测变量矩阵(每一列是一次观测向量):','状态参数',1,{'[0 0.1 0.5 0.8;0 0.42 0.37 0.4]'});
Z_k = str2num(paraZkstr{1});
[rowZk,colZk]  = size(Z_k);
estimateX = zeros(rowZk,colZk);
estimateX_ = zeros(rowZk,colZk);
estimateP_ = cell(1,colZk);
estimateP = cell(1,colZk);
estimateK = cell(1,colZk);
paraAstr = inputdlg('请输入矩阵A:','状态参数',1,{'[0 1;-0.04 -0.04]'});
A = str2num(paraAstr{1});
paraBstr = inputdlg('请输入矩阵B:','状态参数',1,{'[1 0;0 1]'});
B = str2num(paraBstr{1});
paraUstr = inputdlg('请输入控制变量u:','状态参数',1,{'[0 0 0 0;1 1 1 1]'});
u = str2num(paraUstr{1});
paraX0str = inputdlg('请输入初始观测量X_0:','状态参数',1,{'[0;0.1]'});
X_0 = str2num(paraX0str{1});
tryestimateX_(:,1) = A*X_0 + B*u(:,1);
catchmsgbox('矩阵输入的维度不正确!');return;
end
paraP0str = inputdlg('请输入初始协方差P_0:','状态参数',1,{'[1 1e-3;1e-3 1]'});
P_0 = str2num(paraP0str{1});
paraQstr = inputdlg('请输入状态方程噪声协方差Q:','状态参数',1,{'[1 1e-4;1e-4 1]'});
Q = str2num(paraQstr{1});
tryestimateP_{1} = A*P_0*A' + Q;
catchmsgbox('初始先验协方差估计出错!');return;
end
paraHstr = inputdlg('请输入测量矩阵H:','状态参数',1,{'[1 0;0 1]'});
H = str2num(paraHstr{1});
paraRstr = inputdlg('请输入测量噪声协方差R:','状态参数',1,{'[1 1e-4;1e-4 1]'});
R = str2num(paraRstr{1});
tryestimateK{1} = estimateP_{1}*H'/(H*estimateP_{1}*H' + R);
catchmsgbox('初始增益系数估计出错!');return;
endtryestimateX(:,1) = estimateX_(:,1) + estimateK{1}*(Z_k(:,1) - H*estimateX_(:,1));
catchmsgbox('后验状态估计出错!');return;
endtryestimateP{1} = (eye(rowZk) - estimateK{1}*H)*estimateP_{1};
catchmsgbox('后验状态估计出错!');return;
end
%以下开始进行更新等操作
for i = 2:colZkestimateX_(:,i) = A*estimateX(:,i-1) + B*u(:,i);estimateP_{i} = A*estimateP{i-1}*A'+ Q;estimateK{i} = estimateP_{i}*H'/(H*estimateP_{i}*H' + R);estimateX(:,i) = estimateX_(:,i) + estimateK{i}*(Z_k(:,i) - H*estimateX_(:,i));estimateP{i} = (eye(rowZk) - estimateK{i}*H)*estimateP_{i};
end
%以下开始进行绘图等操作
for i = 1:rowZkif rowZk == 1figure(1);elsesubplot(fix(rowZk/2),2,i);endplot(1:colZk,estimateX(i,:),'*','color','b');hold onplot(1:colZk,Z_k(i,:),'-','color','r');xlabel('次数');ylabel('状态值');legend(['状态量',num2str(i)],['测量值',num2str(i)]);grid onhold off
end

经典卡尔曼滤波器直接调用代码(Matlab)相关推荐

卡尔曼滤波器求速度matlab,卡尔曼滤波器算法浅析及matlab实战
原标题:卡尔曼滤波器算法浅析及matlab实战作者:Liu_LongPo 出处:Liu_LongPo的博客卡尔曼滤波器是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的 ...
卡尔曼滤波器原理讲解及其matlab实现
目录一:卡尔曼滤波器的信号模型[1-2] 二:其他方程及变量介绍三:卡尔曼滤波器递推公式四:matlab仿真[3] 参考文献: 引言:在进行一些信号处理的过程中,我们通常会采集到一些数据,但是实 ...
【理解】Kalman卡尔曼滤波器附pythonmatlab代码
一般我是把参考链接放在最后的,但这一次,我放最前排,以表示我对这两位博主的极大感谢,写得真的是太好了! 可能我即将写的这篇是最详细明白的一篇关于卡尔曼滤波器的理解,如果还有疑问,我会补充修正文章目录 ...
由浅入深的扩展卡尔曼滤波器教程
本篇译文翻译自 The Extended Kalman Filter : An Interactive Turorial for Non-Experts. 原文本文惯例及说明 : 译文中的Demo请 ...
卡尔曼滤波器学习笔记（二）
扩展卡尔曼滤波器的原理及应用经典的卡尔曼滤波只适用于线性且满足高斯分布的系统,但实际工程中并不是这么简单,比如飞行器在水平运动时有可能伴随着自身的自旋,此时的系统并不是线性的,这时就需要应用扩展卡尔 ...
优达学城《无人驾驶入门》学习笔记——卡尔曼滤波器实现详解
优达学城<无人驾驶入门>的第二个项目是实现矩阵类,要求通过python编写一个用来计算矩阵的类Matrix.编写这个类并不难,涉及到的线性代数方面的知识也不多,比如矩阵的加法.减法.乘法, ...
【信号处理】基于扩展卡尔曼滤波器和无迹卡尔曼滤波器的窄带信号时变频率估计（Matlab代码实现）
目录 1 概述 2 数学模型 3 运行结果 4 结论 5 参考文献 6 Matlab代码实现 1 概述本文讲解和比较了基于卡尔曼滤波器的频率跟踪方法的能力,例如扩展卡尔曼滤波器 (EKF) 和无味卡 ...
信号去噪，基于Sage-Husa自适应卡尔曼滤波器实现海浪磁场噪声抑制及海浪磁场噪声的产生附Matlab代码
信号去噪,基于Sage-Husa自适应卡尔曼滤波器实现海浪磁场噪声抑制及海浪磁场噪声的产生附Matlab代码信号处理中的一个关键问题就是信号去噪.在实际应用中,很多信号可能会受到环境噪声的干扰,这些 ...
matlab中卡尔曼滤波,卡尔曼滤波器和matlab代码.doc
完美WORD格式专业整理分享信息融合大作业 --维纳最速下降法滤波器,卡尔曼滤波器设计及Matlab仿真时间:2010-12-6 专业:信息工程班级学号:2007302171 姓名:马志强滤 ...

经典卡尔曼滤波器直接调用代码(Matlab)

经典卡尔曼滤波器直接调用代码(Matlab)相关推荐

最新文章

热门文章