卡尔曼滤波学习笔记

卡尔曼滤波学习笔记

线性卡尔曼滤波

线性系统的状态方程和测量方程:

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

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');

扩展卡尔曼滤波

例子：扩展卡尔曼滤波在标量非线性系统中的应用

T=50;%总时间
Q=10;%W(k)的方差
R=1;%V(k)的方差
w=sqrt(Q)*randn(1,T);%产生过程噪声
v=sqrt(R)*randn(1,T);%产生观测噪声
%初始化
x=zeros(1,T);
x(1)=0.1;
y=zeros(1,T);
y(1)=x(1)^2/20+v(1);
Xekf=zeros(1,T);
Xekf(1)=x(1);
Yekf=zeros(1,T);
Yekf(1)=y(1);
P0=eye(1);
 
for k=2:T
    x(k)=0.5*x(k-1)+2.5*x(k-1)/(1+x(k-1)^2)+8*cos(1.2*k)+w(k-1);
    y(k)=x(k)^2/20+v(k);
    %状态预测
    Xn=0.5*Xekf(k-1)+2.5*Xekf(k-1)/(1+Xekf(k-1)^2)+8*cos(1.2*k);
    %观测预测
    Zn=Xn^2/20;
    %求状态矩阵A
    A=0.5+2.5 *(1-Xn^2)/(1+Xn^2)^2;
    %求观测矩阵C
    C=Xn/10;
    %协方差预测
    P=A*P0*A'+Q;
    %卡尔曼增益     
    K=P*C'*inv(C*P*C'+R);
    %状态更新
    Xekf(k)=Xn+K*(y(k)-Zn);
    %协方差阵更新
    P0=(eye(1)-K*C)*P;
end
%误差
Xstd=zeros(1,T);
for k=1:T
    Xstd(k)=abs( Xekf(k)-x(k) );
end
%画图
figure
hold on;box on;
plot(x,'-ko','MarkerFace','g');
plot(Xekf,'-ks','MarkerFace','b');
figure
hold on;box on;
plot(Xstd,'-ko','MarkerFace','g');