版权声明：本文为博主原创文章，遵循 CC 4.0 by-sa 版权协议，转载请附上原文出处链接和本声明。
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');
%%%%%%%%%%%%%%%%%%%%%%%%%
- 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
- 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: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('速度误差均方值')
%%%%%%%%%%%%%%%%%%%%%%%
- 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
- 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: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
%%%%%%%%%%%%%%%%%%%%%%%%
- 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
- 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
%将图像文件读入矩阵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
%目标中心的位置，也就是目标的x，y坐标
figure
plot(cr,'-g*')
hold on
plot(cc,'-r*')
- 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
