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:NX(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:NErr_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');
%%%%%%%%%%%%%%%%%%%%%%%%%

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51

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

%%%%%%%%%%%%%
% 卡尔曼滤波用于自由落体运动目标跟踪问题
%%%%%%%%%%%%%%
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:Nmessure_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('速度误差均方值')
%%%%%%%%%%%%%%%%%%%%%%%

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69
• 70
• 71
• 72
• 73

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

%  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:NX(:,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:NXn=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:NErr_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)<=2dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
elsedist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
end
%%%%%%%%%%%%%%%%%%%%%%%%

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56

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

帧间差法目标检测

% 目标检测函数，这个函数主要完成将目标从背景中提取出来
function detect
clear,clc;
%计算背景图片数目
Imzero = zeros(240,320,3);
for i = 1:5%将图像文件读入矩阵ImIm{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 %没检测到目标，继续下一帧图像continueendhold onfor c = -0.9*radius: radius/20 : 0.9*radiusr = sqrt(radius^2-c^2);plot(cc(i)+c,cr(i)+r,'g.')plot(cc(i)+c,cr(i)-r,'g.')endpause(0.02)
end
%目标中心的位置，也就是目标的x，y坐标
figure
plot(cr,'-g*')
hold on
plot(cc,'-r*')