卡尔曼滤波学习笔记

线性卡尔曼滤波

卡尔曼滤波在温度测量中的应用

clc        %清理命令行
close all;   %关闭当前及所有工作区中所有打开的数据库
clear all;  % 关闭从内存中释放所有的内存变量和数以及所有用户自定义的菜单栏, 菜单, 和窗口的定义

N=120;
CON=25;
Xexpect=CON*ones(1,N);
A=1; %状态转移矩阵
C=1; %测量矩阵

Q=0.01;%W(k)的方差
R=0.25;%V(k)的方差
W=sqrt(Q)*randn(1,N);   %过程噪声
V=sqrt(R)*randn(1,N);   %测量噪声
Xkf=zeros(1,N);         %初始估计值
%%初始化
P(1)=0.01; 
X(1)=25.1;
Y(1)=24.9;
Xkf(1)=Y(1);
I=eye(1); 

for k=2:N
    X(k)=A*X(k-1)+W(k-1);  %状态方程
    Y(k)=C*X(k)+V(k);      %测量方程
    %%时间更新
    X_pre=A*Xkf(k-1);      
    P_pre=A*P(k-1)*A'+Q;
    %%测量更新
    Kg=P_pre*C'*inv(C*P_pre*C'+R);
    Xkf(k)=X_pre+Kg*[Y(k)-C*X_pre]; 
    P(k)=(I-Kg*C)*P_pre;
end

for k=1:N
    Err_Messure(k)=abs(Y(k)-X(k));
    Err_Kalman(k)=abs(Xkf(k)-X(k));
end

t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,Xexpect,'-b',t,X,'-r',t,Y,'-k',t,Xkf,'-g');
legend('expected','real','measure','kalman extimate');         
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');
figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');
legend('messure error','kalman error');         
xlabel('sample time');