kalman 一维递归算法 MATLAB演示

Posted 2022-01-28 haoming Hu

% 情景：对一个物体的长度进行测量
% 变量：
% 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);