Matlab 卡尔曼滤波

% Kalman filter example of temperaturemeasurement in Matlab

% This M code is modified from Xuchen Yao'smatlab on 2013/4/18

%房间当前温度真实值为24度，认为下一时刻与当前时刻温度相同，误差为0.02度（即认为连续的两个时刻最多变化0.02度）。

%温度计的测量误差为0.5度。

%开始时，房间温度的估计为23.5度，误差为1度。

% intial parameters

n_iter = 1000; %计算连续n_iter个时刻

sz = [n_iter, 1]; % size of array. n_iter行，1列

t=-5:0.01:4.99;

tt=0.01*t.^2';

x = 24-tt; %温度的真实值

Q = 4e-4; %过程方差，反应连续两个时刻温度方差。更改查看效果

R = 0.25; %测量方差，反应温度计的测量精度。更改查看效果

z = x + sqrt(R)*randn(sz); % z是温度计的测量结果，在真实值的基础上加上了方差为0.25的高斯噪声。

%对数组进行初始化

xhat=zeros(sz); %对温度的后验估计。即在k时刻，结合温度计当前测量值与k-1时刻先验估计，得到的最终估计值

P=zeros(sz); %后验估计的方差

xhatminus=zeros(sz); %温度的先验估计。即在k-1时刻，对k时刻温度做出的估计

Pminus=zeros(sz); %先验估计的方差

K=zeros(sz); %卡尔曼增益，反应了温度计测量结果与过程模型（即当前时刻与下一时刻温度相同这一模型）的可信程度

% intial guesses

xhat(1) = 23.5-tt(1); %温度初始估计值为23.5度

P(1) =1; %误差方差为1

for k = 2:n_iter

%时间更新（预测）

xhatminus(k) = xhat(k-1)-(tt(k)-tt(k-1)); %用上一时刻的最优估计值来作为对当前时刻的温度的预测

Pminus(k) = P(k-1)+Q; %预测的方差为上一时刻温度最优估计值的方差与过程方差之和

%测量更新（校正）

K(k) = Pminus(k)/( Pminus(k)+R ); %计算卡尔曼增益

xhat(k) = ...

xhatminus(k)+K(k)*(z(k)-xhatminus(k)); %结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差

P(k) = (1-K(k))*Pminus(k); %计算最终估计值的方差

end

FontSize=14;

LineWidth=3;

figure();

plot(z,'k+'); %画出温度计的测量值

hold on;

plot(xhat,'b-','LineWidth',LineWidth) %画出最优估计值

hold on;

plot(x,'g-','LineWidth',LineWidth);

%画出真实值

legend('温度计的测量结果', '后验估计', '真实值');

xl=xlabel('时间(分钟)');

yl=ylabel('温度');

set(xl,'fontsize',FontSize);

set(yl,'fontsize',FontSize);

hold off;

set(gca,'FontSize',FontSize);

figure();

valid_iter = 2:n_iter; % Pminus not validat step 1

plot(valid_iter,P(valid_iter),'LineWidth',LineWidth);

%画出最优估计值的方差 

legend('后验估计的误差估计');

xl=xlabel('时间(分钟)');

yl=ylabel('℃^2');

set(xl,'fontsize',FontSize);

set(yl,'fontsize',FontSize);

set(gca,'FontSize',FontSize);

disp('this is a string');