经典卡尔曼滤波器直接调用代码(Matlab)
程序员文章站
2022-07-12 09:49:41
...
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