MATLAB--卡尔曼滤波

利用matlab来实现卡尔曼滤波，一个简单的例子：

clear all;clc;close all;%*************利用卡尔曼滤波算法来估计状态值**************%%定义状态转移矩阵和观测矩阵
syms x u;
h(x) = x;  %exp(x^3+2*x+2);
H_ = diff(h,x);f(x,u) = x + u;
F_ = diff(f,x);%%首先构造一些观测值
u_k = 0.015;
x_ = 0.01:u_k:1;
Q = 0.01;
R = 0.25;
nosie_Q = sqrt(Q)*randn(1,size(x_,2));%产生高斯噪音
nosie_R = sqrt(R)*randn(1,size(x_,2));%产生高斯噪音x_data = f(x_,0)+nosie_Q;
y_ = h(x_data) + nosie_R;%构造一系列观测值%初始化
x_posterior = 0.01;
P_posterior = 0.04;
x_prior = 0;
P_prior = 0;
K = 0;H = H_;
F = F_;
x_posterior_save = [];
P_posterior_save = [];%卡尔曼滤波过程
for i = 1:size(x_,2)%第一步：预测x_prior = f(x_posterior,u_k);%prior先验，posterior后验P_prior = F(x_posterior,u_k)*P_posterior*F(x_posterior,u_k)' + Q;%第二步：更新H_vpa = vpa(H(x_prior),5);%卡尔曼增益K = P_prior*H_vpa/(H_vpa*P_prior*H_vpa'+ R);K = vpa(K,5);%后验方差P_posterior = (1-K*H_vpa)*P_prior;P_posterior = vpa(P_posterior,5);%后验估计x_posterior = x_prior + K*(y_(i)-h(x_prior));x_posterior = vpa(x_posterior,5);%保存数据x_posterior_save = [x_posterior_save x_posterior];P_posterior_save = [P_posterior_save P_posterior];fprintf('后验x为：%f\n',x_posterior);
end%不经优化直接求解的数值，用于比较
x_pre = [];
for i = 1:size(y_,2)x_pre_ = solve(h==y_(i));x_pre = [x_pre x_pre_(1)];x_pre = vpa(x_pre,4);
endfigure('Name','Y');
%axis([0 1 0 120]);
hold on;
grid on;
plot(x_,y_,'black');
plot(x_,h(x_),'red');
legend('观测值','理论值');  figure('Name','x');
%axis([0 100 -30 140]);
hold on;
grid on;
plot(1:u_k*100:100,x_posterior_save*100,'red',1:u_k*100:100,1:u_k*100:100,'green',1:u_k*100:100,x_data.*100,'blue');
plot(1:u_k*100:100,x_pre*100,'-k');
legend('卡尔曼优化值','理论值','真实值','未优化值');

结果：

1.创建的数据以及理论曲线：

2.后验估计，理论值和真实值

通过比较估计值和真实值可以看出精度还是不错的

MATLAB--卡尔曼滤波相关推荐

【路径规划】基于matlab卡尔曼滤波、三次插值极速赛道赛车路径规划【含Matlab源码 2158期】
一.卡尔曼滤波路径追踪优化简介割草机器人通过比对当前t时刻位置.导航方程之间偏移角度θ和偏移距离d,确定t+1时刻的运动方向属于递推型路径追踪.割草机器人工作过程中受到地面起伏等环境因素影响,在采用 ...
MATLAB卡尔曼滤波-实例
来源于B站老师:DR_CAN 卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法.由于观测数据中包括系统中的噪声和干扰的影 ...
卡尔曼滤波预测matlab,卡尔曼滤波Matlab示例
% 在二维空间,假设运动物体的一组运动数据,用卡尔曼滤波方法进行处理,两点起始法 % 实验室的博客 clear; % 1.1. 实际和测量的运动数据,线性运动 % rx = 0:0.1:10; %x方 ...
matlab做卡尔曼滤波预测,求Matlab卡尔曼滤波预测股票价格的程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%% ...
无人机matlab程序,无人机高度基于matlab卡尔曼滤波程序数据融合
altitude Kalman计算 kalman1 Debug Test1.dsp Test1.dsw Test1.ncb Test1.opt Test1.plg dataK.h kalman.c k ...
【老生谈算法】基于matlab的运动目标识别与跟踪系统设计与算法原理及程序源码——目标识别算法
基于matlab的运动目标识别与跟踪系统设计与算法原理大家好,今天给大家介绍基于matlab的运动目标的监测与跟踪系统设计与原理. 视频图像分析主要是对运动图像序列进行分析处理,它通常涉及到运动检测 ...
matlab 输出字母,MATLAB里面的legend函数输出希腊字母
视觉引导系统在机器人拾取方面的应用视觉引导系统在机器人拾取方面的应用当将自动化组件放置在具有X,Y特征的计划上时,高性能视觉系统就变得至关重要.照亮产品后,视觉引导系统会拾取每个微小的组件,找到其 ...
GNSS/INS组合导航学习-GINAV（一）
从今天开始整理一下,最近半年学习的组合导航算法目前开源程序 1.严老师PSINS工具箱 (MATLAB--卡尔曼滤波算法) 2.GINAV (MATLAB--卡尔曼滤波算法 ) 3.KF-GINS ...
matlab中卡尔曼滤波,卡尔曼滤波器和matlab代码.doc
完美WORD格式专业整理分享信息融合大作业 --维纳最速下降法滤波器,卡尔曼滤波器设计及Matlab仿真时间:2010-12-6 专业:信息工程班级学号:2007302171 姓名:马志强滤 ...
matlab温度数据怎么滤波_卡尔曼滤波算法思想理解 Kalman filter 第一篇
卡尔曼滤波算法思想理解 Kalman filter 第一篇最近在初步的理解目标跟踪的领域, 其中一个非常经典的算法卡尔曼滤波Kalman filter是需要有很好的理解才行, 由于已经脱离了学校,懂 ...

MATLAB--卡尔曼滤波

MATLAB--卡尔曼滤波相关推荐

最新文章

热门文章