kalman 一维递归算法 MATLAB演示

% 情景:对一个物体的长度进行测量
% 变量:
% XK0    :初始估计值
% Xk     :当前估计值
% ZK     :第K次测量值
% ZK0    : 第一次测量值
% EMEA   :第K次测量误差(不考虑系统测量噪声,默认为一个定值)
% EESTK  : 第K次估计误差
% EESTK0 : 初始估计误差(初始给一个定值)
% KK0    :初始卡尔曼增益
% KK     :卡尔曼增益
% 步骤一:计算kalman增益:KK
%       第一次计算:KK0 = EESTK0/(EESTK0+EMEA) 得到初始kalman增益
%       第二次计算:KK  = EESTK/(EESTK+EMEA)   得到往后的每一个kalman增益
% 
% 步骤二:计算当前估计值:XK
%       第一次计算:XK0 = XK0+KK0*(ZK0-XK0)     得到初始估计值
%       第二次计算:XK  = XK + KK(ZK-XK)        得到往后的估计值并更新
% 
% 步骤三:计算估计误差:EESTK
%       EESTK = (1-KK)*EESTK                  得到往后的估计值

% 给定初始值   物体长度真实值为50
XK0 = 40;
EESTK0 = 5;
ZK0 = 51;
EMEA = 3;
% 计算
KK = [100];
XK  = [100];
EESTK = [100];
ZK = [51,48,47,52,51,48,49,53,48,49,52,53,51,52,49,56,50,47,48,53,52,51,51,52,50,46,48,49,48,49,52,53,51,52,49,56,50,47,48,53,48,47,52,51,48,49,53,48,49,52,53,51,52,49,56,50,47,48,53,52,51,51,52,50,46,48,49,48,49,48,47,52,51,48,49,53,48,49,52,53,51,52,49,56,50,47,48,53,52,51,51,52,50,46,48,49,48,49,48,47,52,51,48,49,53,48,49,52,53,51,52,49,56,50,47,48,53,52,51,51,52,50,46,48,49,48,49,48,47,52,51,48,49,53,48,49,52,53,51,52,49,56,50,47,48,53,52,51,51,52,50,46,48,49,48,49];

KK0 = EESTK0/(EESTK0+EMEA);
KK(1) = KK0;
XK(1) = XK0;
EESTK(1) = EESTK0;

for i=2:99
     KK(i) = EESTK(i-1)/(EESTK(i-1)+EMEA);
     XK(i) = XK(i-1) + KK(i-1)*(ZK(i)-XK(i-1));
     EESTK(i) = (1-KK(i))* EESTK(i-1);
end
    

plot(XK);


在这里插入图片描述

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

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值