卡尔曼滤波应用及其matlab实现

Github个人博客：https://joeyos.github.io

线性卡尔曼滤波

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

X(k)=AX(k-1)+TW(k-1)
Z(k)=H*X(k)+V(k)

房间温度在25摄氏度左右，测量误差为正负0.5摄氏度，方差0.25，R=0.25。Q=0.01,A=1,T=1,H=1。

假定快时刻的温度值、测量值为23.9摄氏度，房间真实温度为24摄氏度，温度计在该时刻测量值为24.5摄氏度，偏差为0.4摄氏度。利用k-1时刻温度值测量第k时刻的温度，其预计偏差为:
P(k|k-1)=P(k-1)+Q=0.02
卡尔曼增益k=P(k|k-1) / (P(k|k-1) + R)=0.0741

X(k)=23.9+0.0741*(24.1-23.9)=23.915摄氏度。

k时刻的偏差为P(k)=(1-K*H)P(k|k-1)=0.0186。
最后由X(k)和P(k)得出Z(k+1)。

matlab实现为：

% 程序说明：Kalman滤波用于温度测量的实例
function main
N=120;
CON=25;%房间温度在25摄氏度左右
Xexpect=CON*ones(1,N);
X=zeros(1,N);  
Xkf=zeros(1,N);
Z=zeros(1,N);
P=zeros(1,N); 
X(1)=25.1;
P(1)=0.01;
Z(1)=24.9;
Xkf(1)=Z(1);
Q=0.01;%W(k)的方差
R=0.25;%V(k)的方差
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);
F=1;
G=1;
H=1;
I=eye(1); 
%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N
    X(k)=F*X(k-1)+G*W(k-1);  
    Z(k)=H*X(k)+V(k);
    X_pre=F*Xkf(k-1);           
    P_pre=F*P(k-1)*F'+Q;        
    Kg=P_pre*inv(H*P_pre*H'+R); 
    e=Z(k)-H*X_pre;            
    Xkf(k)=X_pre+Kg*e;         
    P(k)=(I-Kg*H)*P_pre;
end
%%%%%%%%%%%%%%%%
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
for k=1:N
    Err_Messure(k)=abs(Z(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,Z,'-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');
%%%%%%%%%%%%%%%%%%%%%%%%%

卡尔曼滤波在自由落体运动目标跟踪中的应用

%%%%%%%%%%%%%
% 卡尔曼滤波用于自由落体运动目标跟踪问题
%%%%%%%%%%%%%%
function main
N=1000; %仿真时间，时间序列总数
%噪声
Q=[0,0;0,0];%过程噪声方差为0，即下落过程忽略空气阻力
R=1; %观测噪声方差
W=sqrt(Q)*randn(2,N);%既然Q为0，即W=0
V=sqrt(R)*randn(1,N);%测量噪声V(k)
%系数矩阵
A=[1,1;0,1];%状态转移矩阵
B=[0.5;1];%控制量
U=-1;
H=[1,0];%观测矩阵
%初始化
X=zeros(2,N);%物体真实状态
X(:,1)=[95;1];%初始位移和速度
P0=[10,0;0,1];%初始误差
Z=zeros(1,N);
Z(1)=H*X(:,1);%初始观测值
Xkf=zeros(2,N);%卡尔曼估计状态初始化
Xkf(:,1)=X(:,1);
err_P=zeros(N,2);
err_P(1,1)=P0(1,1);
err_P(1,2)=P0(2,2);
I=eye(2); %二维系统
%%%%%%%%%%%%%
for k=2:N
    %物体下落，受状态方程的驱动
    X(:,k)=A*X(:,k-1)+B*U+W(k);
    %位移传感器对目标进行观测
    Z(k)=H*X(:,k)+V(k);
    %卡尔曼滤波
    X_pre=A*Xkf(:,k-1)+B*U;%状态预测 
    P_pre=A*P0*A'+Q;%协方差预测
    Kg=P_pre*H'*inv(H*P_pre*H'+R);%计算卡尔曼增益
    Xkf(:,k)=X_pre+Kg*(Z(k)-H*X_pre);%状态更新
    P0=(I-Kg*H)*P_pre;%方差更新
    %误差均方值
    err_P(k,1)=P0(1,1);
    err_P(k,2)=P0(2,2);
end
%误差计算
messure_err_x=zeros(1,N);%位移的测量误差
kalman_err_x=zeros(1,N);%卡尔曼估计的位移与真实位移之间的偏差
kalman_err_v=zeros(1,N);%卡尔曼估计的速度与真实速度之间的偏差
for k=1:N
    messure_err_x(k)=Z(k)-X(1,k);
    kalman_err_x(k)=Xkf(1,k)-X(1,k);
    kalman_err_v(k)=Xkf(2,k)-X(2,k);
end
%%%%%%%%%%%%%%%
%画图输出
%噪声图
figure
plot(V);
title('messure noise')
%位置偏差
figure
hold on,box on;
plot(messure_err_x,'-r.');%测量的位移误差
plot(kalman_err_x,'-g.');%卡尔曼估计位置误差
legend('测量误差','kalman估计误差')
figureplot(kalman_err_v);
title('速度误差')
figure
plot(err_P(:,1));
title('位移误差均方值')
figure
plot(err_P(:,1));
title('速度误差均方值')
%%%%%%%%%%%%%%%%%%%%%%%

卡尔曼滤波在船舶GPS导航定位

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BauFQwsS-1575269126935)(https://img-blog.csdn.net/20180205210213419?watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQvemhhbmdxdWFuMjAxNQ==/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/SouthEast)]

%  Kalman滤波在船舶GPS导航定位系统中的应用
function main
clc;clear;
T=1;%雷达扫描周期
N=80/T;%总的采样次数
X=zeros(4,N);%目标真实位置、速度
X(:,1)=[-100,2,200,20];%目标初始位置、速度
Z=zeros(2,N);%传感器对位置的观测
Z(:,1)=[X(1,1),X(3,1)];%观测初始化
delta_w=1e-2;%如果增大这个参数，目标真实轨迹就是曲线
Q=delta_w*diag([0.5,1,0.5,1]) ;%过程噪声均值
R=100*eye(2);%观测噪声均值
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
H=[1,0,0,0;0,0,1,0];%观测矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
    X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);%目标真实轨迹
    Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1); %对目标观测
end
%卡尔曼滤波
Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化
P0=eye(4);%协方差阵初始化
for i=2:N
    Xn=F*Xkf(:,i-1);%预测
    P1=F*P0*F'+Q;%预测误差协方差
    K=P1*H'*inv(H*P1*H'+R);%增益
    Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
    P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
end
%误差更新
for i=1:N
    Err_Observation(i)=RMS(X(:,i),Z(:,i));%滤波前的误差
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%滤波后的误差
end
%画图
figure
hold on;box on;
plot(X(1,:),X(3,:),'-k');%真实轨迹
plot(Z(1,:),Z(2,:),'-b.');%观测轨迹
plot(Xkf(1,:),Xkf(3,:),'-r+');%卡尔曼滤波轨迹
legend('真实轨迹','观测轨迹','滤波轨迹')
figure
hold on; box on;
plot(Err_Observation,'-ko','MarkerFace','g')
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
legend('滤波前误差','滤波后误差')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算欧式距离子函数
function dist=RMS(X1,X2);
if length(X2)<=2
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
else
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end
%%%%%%%%%%%%%%%%%%%%%%%%

卡尔曼滤波在视频目标跟踪中的应用

帧间差法目标检测

% 目标检测函数，这个函数主要完成将目标从背景中提取出来
function detect
clear,clc;
%计算背景图片数目
Imzero = zeros(240,320,3);
for i = 1:5
    %将图像文件读入矩阵Im
    Im{i} = double(imread(['DATA/',int2str(i),'.jpg']));
    Imzero = Im{i}+Imzero;
end
Imback = Imzero/5;
[MR,MC,Dim] = size(Imback);
%遍历所有图片
for i = 1 : 60
    %读取所有帧
    Im = (imread(['DATA/',int2str(i), '.jpg']));
    imshow(Im); %显示图像Im,图像对比度低
    Imwork = double(Im);
    %检测目标
    [cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i);
    if flag==0 %没检测到目标，继续下一帧图像
        continue
    end
    hold on
    for c = -0.9*radius: radius/20 : 0.9*radius
        r = sqrt(radius^2-c^2);
        plot(cc(i)+c,cr(i)+r,'g.')
        plot(cc(i)+c,cr(i)-r,'g.')
    end
    pause(0.02)
end
%目标中心的位置，