卡尔曼滤波(Kalman Filtering)——(6)MATLAB仿真(保姆级)
MATLAB仿真
- 一、卡尔曼滤波的实际应用
- 二、流程图
- 三、执行过程
- 四、程序代码
- 五、仿真结果
- 参考文献
一、卡尔曼滤波的实际应用
在这里依旧以前面提到的测量硬币为例进行MATLAB仿真。现有一枚硬币为了这枚硬币的直径,我们进行了多次测量,但是所使用的的尺子存在一定误差,人进行测量的过程中存在测量误差,而且由常识可以估算硬币的直径得到估计值。所以测量所得到的值与估计值哪一个更接近真实值呢?
已知:硬币直径真实值为50mm;
首次估计误差为40mm;
过程误差方差为4e-4;
尺子的误差方差为3。
二、流程图
由前面几篇博客对卡尔曼滤波公式进行了仔细的推导,一共得到了五大公式。卡尔曼滤波的过程就是对系统进行预测加矫正的过程,两者循环迭代使系统逐渐趋近于真实值。下面就是卡尔曼滤波过程的流程图。
三、执行过程
一、由k-1次估计的硬币直径x^k−1\hat{x}_{k-1}x^k−1 去估计第k次系统的状态值(直径) x^k−,x^k−=Ax^k−1+Buk−1\hat{x}_{k}^{-}, \hat{x}_{k}^{-}=A \hat{x}_{k-1}+B u_{k-1}x^k−,x^k−=Ax^k−1+Buk−1, 对应于本例中系统无输入uk−1=0u_{k-1} = 0uk−1=0 ;A=H=I;Q=4e−4;R=3A = H= I;Q = 4e-4;R = 3A=H=I;Q=4e−4;R=3。则x^k−=x^k−1=40mm\hat{x}_{k}^{-}=\hat{x}_{k-1}=40mmx^k−=x^k−1=40mm。
二、由上一次的误差协方差 Pk−1P_{k-1}Pk−1 和过程噪声Q预测新的误差 Pk−,Pk−=P_{k}^{-}, P_{k}^{-}=Pk−,Pk−= APk−1AT+QA P_{k-1} A^{T}+QAPk−1AT+Q, 对应于本例中Pk−=5P_{k}^{-}=5Pk−=5 。
三、计算卡尔曼增益, Kk=Pk−HT(HPk−HT+R)−1K_{k}=P_{k}^{-} H^{T}\left(H P_{k}^{-} H^{T}+R\right)^{-1}Kk=Pk−HT(HPk−HT+R)−1, 对应于本例中 Kk=K_{k}=Kk= 5/(5+3),Kk=0.6255/\left(5+3\right), \quad K_{k}=0.6255/(5+3),Kk=0.625 。
四、 进行校正更新, x^k=x^k−+Kk(zk−Hx^k−)\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)x^k=x^k−+Kk(zk−Hx^k−), 对应于上例中 x^k=40+0.625∗\hat{x}_{k}=40+0.625 *x^k=40+0.625∗ (zk−40)(z_{k}-40)(zk−40), 此 x^k\hat{x}_{k}x^k 即为 k\mathrm{k}k 时刻的最优直径。
五、为下一步估计 k+1\mathrm{k}+1k+1 时刻的最优温度值的迭代进行更新操作, 级更新 PkP_{k}Pk 值, Pk=(I−KkH)Pk−,P_{k}=\left(I-K_{k} H\right) P_{k}^{-}, \quadPk=(I−KkH)Pk−, 对应于上例中的 (1−Kk)∗5=1.875\left(1-K_{k}\right) * 5=1.875(1−Kk)∗5=1.875。
方法 | kalman 公式 | 对应本例kalman 公式 |
---|---|---|
先验估计 | x^k−=Ax^k−1+Buk−1\hat{x}_{k}^-=A \hat{x}_{k-1}+B u_{k-1}x^k−=Ax^k−1+Buk−1 | x^k−=x^k−1\hat{x}_{k}^{-}=\hat{x}_{k-1}x^k−=x^k−1 |
先验误差协方差矩阵 | Pk−=APk−1AT+QP_{k}^{-}=A P_{k-1} A^{T}+QPk−=APk−1AT+Q | Pk−=P_{k}^{-}=Pk−= Pk−1+QP_{k-1} +QPk−1+Q |
卡尔曼增益 | Kk=Pk−HTHPk−HT+RK_{k}=\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T}+R}Kk=HPk−HT+RPk−HT | Kk=Pk−Pk−+RK_{k}=\frac{P_{k}^{-} }{P_{k}^{-}+R}Kk=Pk−+RPk− |
后验估计 | x^k=x^k−+Kk(zk−Hx^k−)\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)x^k=x^k−+Kk(zk−Hx^k−) | x^k=x^k−+Kk(zk−x^k−)\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}- \hat{x}_{k}^{-}\right)x^k=x^k−+Kk(zk−x^k−) |
更新误差协方差矩阵 | Pk=(I−KkH)Pk−P_{k}=\left(I-K_{k} H\right) P_{k}^{-}Pk=(I−KkH)Pk− | Pk=(I−Kk)Pk−P_{k}=\left(I-K_{k} \right) P_{k}^{-}Pk=(I−Kk)Pk− |
四、程序代码
本次程序运行环境为WIN7+MATLAB R2018a
% Kalman filter example
%% 系统描述:
% 1.硬币直径真实值为50mm;
% 2.开始时,硬币直径的估计为40mm,估计误差为5mm;
% 3.尺子的测量误差为3mm;
%% 变量初始化
close all;% intial parameters
% 计算连续n_iter次数
n_iter = 100;
% size of array. n_iter行,1列
sz = [n_iter, 1];
% 硬币直径的真实值
x = 50;
% 过程方差,反应连续两个次直径方差。更改查看效果
Q = 4e-4;
% 测量方差,反应尺子的测量精度。更改查看效果
R = 3;
% z是尺子的测量结果,在真实值的基础上加上了方差为3的高斯噪声。
z = x + sqrt(R)*randn(sz);
%% 对数组进行初始化
% 对直径的后验估计。即在k次,结合尺子当前测量值与k-1次先验估计,得到的最终估计值
xhat = zeros(sz);
% 后验估计的方差
P = zeros(sz);
% 直径的先验估计。即在k-1次,对k时刻直径做出的估计
xhatminus = zeros(sz);
% 先验估计的方差
Pminus = zeros(sz);
% 卡尔曼增益,反应了尺子测量结果与过程模型(即当前时刻与下一次直径相同这一模型)的可信程度
K = zeros(sz);
% intial guesses
xhat(1) = 40; %直径初始估计值为40mm
P(1) =10; % 误差方差为10
%% kalman 方程
for k = 2:n_iter% 时间更新(预测) % 用上一次的最优估计值来作为对本次的直径的预测xhatminus(k) = xhat(k-1);% 预测的方差为上一次直径最优估计值的方差与过程方差之和Pminus(k) = P(k-1)+Q;% 测量更新(校正)% 计算卡尔曼增益K(k) = Pminus(k)/( Pminus(k)+R );% 结合当前时刻尺子的测量值,对上一次的预测进行校正,得到校正后的最优估计。该估计具有最小均方差xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));% 计算最终估计值的方差P(k) = (1-K(k))*Pminus(k);
end
%% 作图
FontSize = 14;
LineWidth = 3; % 线宽
figure();
plot(z,'r-*'); %画出尺子的测量值
hold on;
plot(xhat,'b-','LineWidth',LineWidth) %画出最优估计值
hold on;
plot(x*ones(sz),'g-','LineWidth',LineWidth); %画出真实值
grid on;legend('尺子的测量结果', '后验估计', '真实值');
title('kalman 滤波','fontsize',FontSize);
xl = xlabel('次数');
yl = ylabel('直径(mm)');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
hold off;
set(gca,'FontSize',FontSize);% gca:坐标轴序号figure();
valid_iter = 2:n_iter; % Pminus not valid at step 1
% 画出最优估计值的方差
plot(valid_iter,P(valid_iter),'LineWidth',LineWidth);
grid on;legend('后验估计的误差估计');
title('最优估计值的方差','fontsize',FontSize);
xl = xlabel('次数');
yl = ylabel('次数^2');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
set(gca,'FontSize',FontSize);
五、仿真结果
参考文献
MATLAB官方文档
二维图绘制官方文档
卡尔曼滤波(Kalman Filtering)——(6)MATLAB仿真(保姆级)相关推荐
- 卡尔曼滤波 | Matlab实现卡尔曼滤波(Kalman Filtering)仿真
文章目录 效果一览 文章概述 研究内容 程序设计 参考资料 效果一览 文章概述 Matlab实现卡尔曼滤波(Kalman Filtering)仿真 研究内容 卡尔曼滤波(Kalman filterin ...
- ukf源程序 matlab,《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.1UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 无迹Kalman滤波在目标跟踪中的应用 % ...
- 光伏发电并网逆变simulink/matlab仿真 两级三相/单相系统
光伏发电并网逆变simulink/matlab仿真 两级三相/单相系统 前级采用boost升压斩波电路 mppt最大功率点跟踪采用扰动观察法 可接单相或者三相并网逆变 id=666229534115& ...
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.2UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 功能说明: UKF在目标跟踪中的应用 % 参数 ...
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.3UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明:对比UKF与EKF在非线性系统中应用 ...
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明:练习使用plot函数,线型设置 % ...
- 卡尔曼滤波 - Kalman filtering
百科定义 卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法.由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可 ...
- FPGA 数字信号处理之 FSK 调制、解调的实现与仿真基于 verilog + ise + modelsim + matlab (保姆级)
试验说明: 笔者复现FSK调制.解调的过程中,在网上查了很多很多相关文章,至少在本文之前,没发现能在参考一篇博文的情况下能完整复现实验结果的.总之,全乎的基本没有. 本笔记(文)的目标是让入门者或者说 ...
- 卡尔曼滤波Kalman Filtering:介绍
本文是Quantitative Methods and Analysis: Pairs Trading此书的读书笔记. 控制理论(control theory)是工程学的分支之一,主要应对工程系统控制 ...
- java 二维卡尔曼滤波_卡尔曼滤波 – Kalman Filtering
1. 什么是卡尔曼滤波器 (What is the Kalman Filter?) 在学习卡尔曼滤波器之前,首先看看为什么叫"卡尔曼".跟其他著名的理论(例如傅立叶变换,泰勒 ...
最新文章
- python 函数 默认参数
- SkinRibbonGalleryBarItem添加无Item问题
- 970计算机技术基础,2017年武汉大学测绘遥感信息工程国家重点实验室970计算机技术基础考研强化模拟题...
- 设计模式——工厂模式
- linux日志服务是哪个,『学了就忘』Linux日志管理 — 2.日志服务rsyslogd
- AutoMapper在asp.netcore中的使用
- sql server 群集_SQL Server中的Microsoft群集
- 神经网络画图-ConvNetDraw(简单实用)
- 卡塔兰数(Catalan)
- RN调试利器——React Native Debugger
- auto_ptr 源码 极其解析
- 华为命令 hybird实验
- 深度学习:知识回收(神经网络模型:BPNN原理)
- Python 列表与元组
- java程序员修炼教学视频,快来看鸭~
- WPF定制实现自己的分页控件并配合DataGrid使用
- 港科夜闻|香港科大陈启峰教授:人工智能与深度学习交织-从竞赛开始,到无尽钻研...
- 计算机简历800字,我的简历作文800字
- 重庆南川金佛山中医院“扶正消瘤贴”专家评估研讨会圆满落幕
- 盘点 11 月火火火火的 GitHub 项目