经典卡尔曼滤波器直接调用代码(Matlab)

clc,clear;
%以下开始进行交互式输入等操作
paraZkstr = inputdlg('请输入观测变量矩阵(每一列是一次观测向量):','状态参数',1,{'[0 0.1 0.5 0.8;0 0.42 0.37 0.4]'});
Z_k = str2num(paraZkstr{1}); 
[rowZk,colZk]  = size(Z_k);
estimateX = zeros(rowZk,colZk);
estimateX_ = zeros(rowZk,colZk);
estimateP_ = cell(1,colZk);
estimateP = cell(1,colZk);
estimateK = cell(1,colZk);
paraAstr = inputdlg('请输入矩阵A:','状态参数',1,{'[0 1;-0.04 -0.04]'});
A = str2num(paraAstr{1});
paraBstr = inputdlg('请输入矩阵B:','状态参数',1,{'[1 0;0 1]'});
B = str2num(paraBstr{1});  
paraUstr = inputdlg('请输入控制变量u:','状态参数',1,{'[0 0 0 0;1 1 1 1]'}); 
u = str2num(paraUstr{1});
paraX0str = inputdlg('请输入初始观测量X_0:','状态参数',1,{'[0;0.1]'}); 
X_0 = str2num(paraX0str{1}); 
try
    estimateX_(:,1) = A*X_0 + B*u(:,1);
catch
    msgbox('矩阵输入的维度不正确!');
    return;
end 
paraP0str = inputdlg('请输入初始协方差P_0:','状态参数',1,{'[1 1e-3;1e-3 1]'});
P_0 = str2num(paraP0str{1}); 
paraQstr = inputdlg('请输入状态方程噪声协方差Q:','状态参数',1,{'[1 1e-4;1e-4 1]'});
Q = str2num(paraQstr{1}); 
try
    estimateP_{1} = A*P_0*A' + Q;
catch
    msgbox('初始先验协方差估计出错!');
    return;
end
paraHstr = inputdlg('请输入测量矩阵H:','状态参数',1,{'[1 0;0 1]'});
H = str2num(paraHstr{1});
paraRstr = inputdlg('请输入测量噪声协方差R:','状态参数',1,{'[1 1e-4;1e-4 1]'});
R = str2num(paraRstr{1});
try
   estimateK{1} = estimateP_{1}*H'/(H*estimateP_{1}*H' + R);
catch
    msgbox('初始增益系数估计出错!');
    return; 
end

try
   estimateX(:,1) = estimateX_(:,1) + estimateK{1}*(Z_k(:,1) - H*estimateX_(:,1));
catch
    msgbox('后验状态估计出错!');
    return; 
end

try
   estimateP{1} = (eye(rowZk) - estimateK{1}*H)*estimateP_{1};
catch
    msgbox('后验状态估计出错!');
    return; 
end
%以下开始进行更新等操作
for i = 2:colZk
    estimateX_(:,i) = A*estimateX(:,i-1) + B*u(:,i);
    estimateP_{i} = A*estimateP{i-1}*A'+ Q;
    estimateK{i} = estimateP_{i}*H'/(H*estimateP_{i}*H' + R);
    estimateX(:,i) = estimateX_(:,i) + estimateK{i}*(Z_k(:,i) - H*estimateX_(:,i));
    estimateP{i} = (eye(rowZk) - estimateK{i}*H)*estimateP_{i};
end
%以下开始进行绘图等操作
for i = 1:rowZk
    if rowZk == 1
        figure(1);
    else
        subplot(fix(rowZk/2),2,i);
    end
    plot(1:colZk,estimateX(i,:),'*','color','b');
    hold on
    plot(1:colZk,Z_k(i,:),'-','color','r');
    xlabel('次数');
    ylabel('状态值');
    legend(['状态量',num2str(i)],['测量值',num2str(i)]);
    grid on
    hold off
end