卡尔曼滤波学习笔记1-Matlab模拟温度例子

KF是根据上一状态的估计值和当前状态的观测值推出当前状态的估计值的滤波方法



温度模拟参数选取

xk
系统状态
实际温度
A
系统矩阵
温度不变,为1
B、uk
状态的控制量
无控制量,为0
Zk
观测值
温度计读数
H
观测矩阵
直接读出,为1
wk
过程噪声
温度变化偏差,常量1e-1
vk
测量噪声
读数误差,常量1e-6
Matlab模拟

clear 
  N=200; 
  %w(1)=0; 
  %w=randn(1,N); 
  w=0;                  %系统控制矩阵
  x(1)=0; 
  a=1;                  %温度模拟A为1 
  V=randn(1,N); 
  q1=std(V); 
  %Rvv=q1.^2; 
  Rvv=0.1;              %测量过程协方差 温度模拟R为1e-1
  q2=std(x); 
  Rxx=q2.^2; 
  q3=std(w); 
  %Rww=q3.^2; 
  Rww = 0.000001;       %温度模拟Q为1e-6 
  %c=0.6; 
  c=1;                  %温度模拟H为1
  for k=1:N;
  Y(k)=25+sqrt(0.1)*randn(1);  %温度模拟平均温度为25度 方差(协方差)为0.1的温度输入 测量方程,其中V为测量系统的噪声,c为测量系统的参数 
  end
  p(1)=10;              %协方差 初始值
  s(1)=1;               %最优估计 初始值
  for t=2:N; 
  s(t)=a*s(t-1)+w;                 %先验估计 求当前时刻的估计值  
  p1(t)=a.^2*p(t-1)+Rww;           %协方差估计 求当前时刻的估计值的偏差,a为系统参数,没有控制量,所以没有参数b,Rww为噪声 
  b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);   %求Kg,b(t)为Kg,即Kalman增量 
  s(t)=a*s(t)+b(t)*(Y(t)-a*c*s(t));%后验估计 求t时刻的最优值,即当前时刻的最优值 
  p(t)=p1(t)-c*b(t)*p1(t);         %后验协方差 求当前最状态最优值的偏差,即式子5:(1-c*b(t))*p1(t) 
  end

  figure(1);
  plot(Y,'g--');hold on;
  plot(s,'r--');hold on;


  • 0
    点赞
  • 14
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值