一、简介
1 卡尔曼滤波是什么

卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声，观测值不准确，卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导，会让人很头疼，难以把握其中的主线和思想。所以我参考了国外一位学者的文章，讲述卡尔曼滤波的工作原理，然后编写了一个基于OpenCV的小程序给大家做一下说明。
2 卡尔曼滤波能做什么

假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的：



这辆车可以在荒野移动，为了便于对它进行控制，需要知道它的位置以及移动速度。所以，建立一个向量，用来存储小车的位置和速度



其实，一个系统的状态有很多，选择最关心的状态来建立这个状态向量是很重要的。例如，状态还有水库里面水位的高低、炼钢厂高炉内的温度、平板电脑上面指尖触碰屏幕的位置等等这些需要持续跟踪的物理量。好了，回归到正题，小车上面安装了GPS传感器，这个传感器的精度是10米。但是如果小车行驶的荒野上面有河流和悬崖的话，10米的范围就太大，很容易掉进去进而无法继续工作。所以，单纯靠GPS的定位是无法满足需求的。另外，如果有人说小车本身接收操控着发送的运动指令，根据车轮所转动过的圈数时能够知道它走了多远，但是方向未知，并且在路上小车打滑车轮空转的现象绝对是不可避免。所以，GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息，这些信息包含了很多的和不确定性。如果将所有这些信息综合起来，是否能够通过计算得到我们更想要的准确信息呢？答案是可以的！



3 卡尔曼滤波的工作原理

1.先验状态估计

以之前我们创建的状态变量为例，



下图表示的是一个状态空间图，为了研究方便，假如小车在一条绝对笔直的线路上面行驶，其位置和速度的方向是确定的，不确定的是大小。






二、源代码
clear;
clc;
%采样点的个数
N=228;
%测试数据：纬度
latitude=load('C:\Users\lenovo\Desktop\基于MATLAB的运动轨迹预测，卡尔曼滤波实现\latitude.txt');
%真实维度值
lat=latitude;
%卡尔曼滤波处理的状态，即估计值
lat_kf=zeros(1,N);
%测报值
lat_z=zeros(1,N);
P=zeros(1,N);
%初始纬度值
lat(1)=29.8131;
%初始值的协方差
P(1)=0.09;
%初始测报值
lat_z(1)=29.8027;
%初始估计状态。假设和初始测报值相同
lat_kf(1)=lat_z(1);
%噪声方差
%系统噪声方差
Q=0.1;
%测量噪声方差
R=0.001;
%方差决定噪声大小
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);
%系统矩阵
F=1;
G=1;
H=1;
%本系统状态为1维
I=eye(1);
%模拟纬度测报，并滤波
for k=2:N
    %随时间推移，飞行纬度逐渐变化
    %k时刻的真是纬度值是测报仪器不知道的，测报值可能是无限接近于真实值，但并不是真实值
    %lat(k)=F*lat(k-1)+G*W(k-1);
    %纬度在k时刻的测报值
    lat_z(k)=H*lat(k)+V(k);
    %kalman滤波
    %有了k时刻的测报值lat_z(k)和k-1时刻的状态，那么就可以进行滤波了
    %状态预测
    lat_pre=F*lat_kf(k-1);
    %协方差预测
    P_pre=F*P(k-1)*F'+Q;
    %计算卡尔曼增益
    Kg=P_pre*inv(H*P_pre*H'+R);
    %新息
    e=lat_z(k)-H*lat_pre;
    %状态更新
    lat_kf(k)=lat_pre+Kg*e;
    %协方差更新
    P(k)=(I-Kg*H)*P_pre;
end
%计算误差
%测量值与真实值之间的偏差
Err_Messure=zeros(1,N);
%kalman估计与真实值之间的偏差
Err_Kalman=zeros(1,N);
for k=1:N
    Err_Messure(k)=abs(lat_z(k)-lat(k));
    Err_Kalman(k)=abs(lat_kf(k)-lat(k));
end
t=1:N;

三、运行结果



四、备注
完整代码或者代写添加QQ1564658423

一、简介
1 卡尔曼滤波是什么

卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声，观测值不准确，卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导，会让人很头疼，难以把握其中的主线和思想。所以我参考了国外一位学者的文章，讲述卡尔曼滤波的工作原理，然后编写了一个基于OpenCV的小程序给大家做一下说明。
2 卡尔曼滤波能做什么

假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的：



这辆车可以在荒野移动，为了便于对它进行控制，需要知道它的位置以及移动速度。所以，建立一个向量，用来存储小车的位置和速度



其实，一个系统的状态有很多，选择最关心的状态来建立这个状态向量是很重要的。例如，状态还有水库里面水位的高低、炼钢厂高炉内的温度、平板电脑上面指尖触碰屏幕的位置等等这些需要持续跟踪的物理量。好了，回归到正题，小车上面安装了GPS传感器，这个传感器的精度是10米。但是如果小车行驶的荒野上面有河流和悬崖的话，10米的范围就太大，很容易掉进去进而无法继续工作。所以，单纯靠GPS的定位是无法满足需求的。另外，如果有人说小车本身接收操控着发送的运动指令，根据车轮所转动过的圈数时能够知道它走了多远，但是方向未知，并且在路上小车打滑车轮空转的现象绝对是不可避免。所以，GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息，这些信息包含了很多的和不确定性。如果将所有这些信息综合起来，是否能够通过计算得到我们更想要的准确信息呢？答案是可以的！



3 卡尔曼滤波的工作原理

1.先验状态估计

以之前我们创建的状态变量为例，



下图表示的是一个状态空间图，为了研究方便，假如小车在一条绝对笔直的线路上面行驶，其位置和速度的方向是确定的，不确定的是大小。






二、源代码
% compute the background image
Im0 = double(imread('ball00000100.jpg','jpg'));
Im1 = double(imread('ball00000101.jpg','jpg'));
Im2 = double(imread('ball00000102.jpg','jpg'));
Im3 = double(imread('ball00000103.jpg','jpg'));
Im4 = double(imread('ball00000104.jpg','jpg'));
Imback = (Im0 + Im1 + Im2 + Im3 + Im4)/5;
[MR,MC,Dim] = size(Imback);

% Kalman filter initialization
R=[[0.2845,0.0045]',[0.0045,0.0455]'];
H=[[1,0]',[0,1]',[0,0]',[0,0]'];
Q=0.01*eye(4);
P = 100*eye(4);
dt=1;
A=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]'];
g = 9.8; % pixels^2/time step
Bu = [0,0,0,g]'; 
kfinit=0;
x=zeros(100,4);

% loop over all images
fig1=1;
fig2=0;
fig15=0;
fig3=0;
fig4=4;
for i = 1 : 60
  % load image
  if i < 11
    Im = (imread(['ball0000010',int2str(i-1), '.jpg'],'jpg')); 
  else
    Im = (imread(['ball000001',int2str(i-1), '.jpg'],'jpg')); 
  end
  if fig1 > 0
    figure(fig1)%creat figure with handle fig1
    clf%clear current figure
    imshow(Im)
  end
  Imwork = double(Im);

  %extract ball
  [cc(i),cr(i),radius,flag]=extractball(Imwork,Imback,fig1,fig2,fig3,fig15,i);
  if flag==0
    continue
  end

  if fig1 > 0
    figure(fig1)
    hold on
    
%plot the figure of measure value
%     for c = -0.97*radius: radius/20 : 0.97*radius
%       r = sqrt(radius^2-c^2);
%       plot(cc(i)+c,cr(i)+r,'g.')
%       plot(cc(i)+c,cr(i)-r,'g.')
%     end


    %eval(['saveas(gcf,''TRACK/trk',int2str(i-1),'.jpg'',''jpg'')']);  
  end

  % Kalman update
i
  if kfinit==0
    xp = [MC/2,MR/2,0,0]'
  else
    xp=A*x(i-1,:)' + Bu
  end
  kfinit=1;
  PP = A*P*A' + Q
  K = PP*H'*inv(H*PP*H'+R)
  x(i,:) = (xp + K*([cc(i),cr(i)]' - H*xp))';
x(i,:)
[cc(i),cr(i)]
  P = (eye(4)-K*H)*PP

  if fig1 > 0
    figure(fig1)
    hold on
    for c = -0.97*radius: radius/20 : 0.97*radius
      r = sqrt(radius^2-c^2);
      plot(x(i,1)+c,x(i,2)+r,'r.')
      plot(x(i,1)+c,x(i,2)-r,'r.')
    end
%    eval(['saveas(gcf,''KFILT/kflt',int2str(i-1),'.jpg'',''jpg'')']);  
  end
  % compute the background image
Im0 = double(imread('ball00000100.jpg','jpg'));
Im1 = double(imread('ball00000101.jpg','jpg'));
Im2 = double(imread('ball00000102.jpg','jpg'));
Im3 = double(imread('ball00000103.jpg','jpg'));
Im4 = double(imread('ball00000104.jpg','jpg'));
Imback = (Im0 + Im1 + Im2 + Im3 + Im4)/5;
[MR,MC,Dim] = size(Imback);

% Kalman filter static initializations
R=[[0.2845,0.0045]',[0.0045,0.0455]'];
H=[[1,0]',[0,1]',[0,0]',[0,0]'];
Q=0.01*eye(4);
dt=1;
A1=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,0,0,0]'];  % on table, no vertical velocity
A2=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]']; % bounce
A3=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]']; % normal motion
g = 6.0;              % gravity=pixels^2/time step
Bu1 = [0,0,0,0]';   % on table, no gravity
Bu2 = [0,0,0,g]';   % bounce
Bu3 = [0,0,0,g]';   % normal motion
loss=0.7;

% multiple condensation states
NCON=100;          % number of condensation samples
MAXTIME=60;        % number of time frames
x=zeros(NCON,MAXTIME,4);      % state vectors
weights=zeros(NCON,MAXTIME);  % est. probability of state
trackstate=zeros(NCON,MAXTIME);         % state=1,2,3;
P=zeros(NCON,MAXTIME,4,4);    % est. covariance of state vec.
for i = 1 : NCON              % initialize estimated covariance
for j = 1 : MAXTIME
  P(i,j,1,1) = 100;
  P(i,j,2,2) = 100;
  P(i,j,3,3) = 100;
  P(i,j,4,4) = 100;
end
end
pstop=0.05;      % probability of stopping vertical motion
pbounce=0.30;    % probability of bouncing at current state (overestimated)
xc=zeros(4,1);   % selected state
TP=zeros(4,4);   % predicted covariance

% loop over all images
fig1=1;
fig2=0;
fig15=0;
fig3=0;
for i = 1 : MAXTIME
i
  % load image
  if i < 11
    Im = (imread(['ball0000010',int2str(i-1), '.jpg'],'jpg')); 
  else
    Im = (imread(['ball000001',int2str(i-1), '.jpg'],'jpg')); 
  end
  if fig1 > 0
    figure(fig1)
    clf
    imshow(Im)
  end
  Imwork = double(Im);

  % extract ball
  [cc(i),cr(i),radius,flag]=extractball(Imwork,Imback,fig1,fig2,fig3,fig15,i);
  if flag==0
    for k = 1 : NCON
      x(k,i,:) = [floor(MC*rand(1)),floor(MR*rand(1)),0,0]';
      weights(k,i)=1/NCON;
    end
    continue
  end

  % display green estimated ball circle over original image
  if fig1 > 0
    figure(fig1)
    hold on
    for c = -0.99*radius: radius/10 : 0.99*radius
      r = sqrt(radius^2-c^2);
      plot(cc(i)+c,cr(i)+r,'g.')
      plot(cc(i)+c,cr(i)-r,'g.')
    end
  end

  % condensation tracking
  % generate NCON new hypotheses from current NCON hypotheses
  % first create an auxiliary array ident() containing state vector j
  % SAMPLE*p_k times, where p is the estimated probability of j
  if i ~= 1
    SAMPLE=10;
    ident=zeros(100*SAMPLE,1);
    idcount=0;
    for j = 1 : NCON    % generate sampling distribution
      num=floor(SAMPLE*100*weights(j,i-1));  % number of samples to generate
      if num > 0
        ident(idcount+1:idcount+num) = j*ones(1,num);
        idcount=idcount+num;
      end
    end
  end

  % generate NCON new state vectors
  for j = 1 : NCON

    % sample randomly from the auxiliary array ident()
    if i==1 % make a random vector
      xc = [floor(MC*rand(1)),floor(MR*rand(1)),0,0]';
    else
      k = ident(ceil(idcount*rand(1))); % select which old sample
      xc(1) = x(k,i-1,1);  % get its state vector
      xc(2) = x(k,i-1,2);
      xc(3) = x(k,i-1,3);
      xc(4) = x(k,i-1,4);

      % sample about this vector from the distribution (assume no covariance)
      for n = 1 : 4
        xc(n) = xc(n) + 5*sqrt(P(j,i-1,n,n))*randn(1);
      end
    end

三、运行结果



四、备注
完整代码或者代写添加QQ 1564658423

 

卡尔曼滤波法是一种时域方法，对于具有高斯分布的噪声的线性系统，应用该方法可以得到系统状态的递推最小均方差估计，该方法用状态方程（表示一个状态的转化）描述动力学系统的动态模型（即状态空间转移模型），用观测方程描述动力学的系统观测模型，可处理动态时变系统、非平稳信号和多为信号。

 









 

 

 

 

[学习slam]基于opencv卡尔曼滤波(KalmanFilter)理论与实践

https://blog.csdn.net/KYJL888/article/details/94024509

 


%% % 附matlab下面的kalman滤波程序：
clear all;
% Kalman滤波技术
A=1;                                        % 状态转移矩阵 Φ(k)
H=0.2;                                      % 观测矩阵 H(k)
X(1)=0;                                     % 目标的状态向量 X(k)
% V(1)=0;                                   % 过程噪声 V(k)
Y(1)=1;                                     % 一步预测x(k)的更新 X(k+1|k+1)
P(1)=10;                                    % 一步预测的协方差 P(k)
N=200;                                      %产生一个均值为0，方差为1的1*n维向量（白噪声、正态分布而非均匀分布）
V=randn(1,N);                               % 模拟产生过程噪声(高斯分布的随机噪声)
w=randn(1,N);                               % 模拟产生测量噪声

for k=2:N
    X(k) = A * X(k-1)+V(k-1);               % 状态方程:X(k+1)=Φ(k)X(k)+G(k)V(k),其中G(k)=1
end

Q=std(V)^2;                                 % W(k)的协方差,std()函数用于计算标准偏差     %标准差^2
R=std(w)^2;                                 % V(k)的协方差 covariance

Z=H*X+w;                                    % 观测方程:Z(k+1)=H(k+1)X(k+1)+W(k+1),Z(k+1)是k+1时刻的观测值

for t=2:N
    P(t) = A * P(t-1)+Q;                    % 一步预测的协方差 P(k+1|k)   
    S(t) = H.^2 * P(t)+R;                   % 观测向量的预测误差协方差 S(k+1)
    K(t) = H * P(t)/S(t);                   % 卡尔曼滤波器增益 K(k+1) 
    v(t) = Z(t) - ( A * H * Y(t-1) );       % 新息/量测残差 v(k+1)
    Y(t)=A * Y(t-1) + K(t) * v(t);          % 状态更新方程 X(k+1|k+1)=X(k+1|k)+K(k+1)*v(k+1)  结果
    P(t)=(1-H * K(t)) * P(t);               % 误差协方差的更新方程: P(k+1|k+1)=(I-K(k+1)*H(k+1))*P(k+1|k)
end

t=1:N;
plot(t,Y,'r',t,Z,'g',t,X,'b');              % 红色线最优化估算结果滤波后的值，%绿色线观测值，蓝色线预测值
legend('Kalman滤波结果','观测值','预测值');

 

 

%%
%扩展Kalman滤波在目标跟踪中的应用 	
%function EKF_For_TargetTracking
clc;clear;	close all
T=1;%雷达扫描周期	
N=60/T;%总的采样次数	
X=zeros(4,N);%目标真实位置、速度
X(:,1)=[-100,2,200,20];%目标初始位置、速度
Z=zeros(1,N);%传感器对位置的观测
delta_w=1e-3;%如果增大这个参数，目标的真实轨迹就是曲线了
Q=delta_w*diag([0.5,1]);%过程噪声方差
G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵	
R=5;%观测噪声方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
x0=200;%观测站的位置
y0=300;
Xstation=[x0,y0];
for t=2:N
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹
end
	
for t=1:N	
    Z(t)=Dist(X(:,t),Xstation)+sqrtm(R)*randn;%观测值	
end	
%EKF	
Xekf=zeros(4,N);
Xekf(:,1)=X(:,1);%Kalman滤波的状态初始化
P0=eye(4);%误差协方差矩阵的初始化
for i=2:N
    Xn=F*Xekf(:,i-1);%一步预测
    P1=F*P0*F'+G*Q*G';%预测误差协方差
    dd=Dist(Xn,Xstation);%观测预测
    %求解雅克比矩阵H
    H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%泰勒展开的一阶近似
    K=P1*H'*inv(H*P1*H'+R);%卡尔曼最优增益
    Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
    P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
end
%误差分析
for i=1:N
    Err_KalmanFilter(i)=Dist(X(:,i),Xekf(:,i));%
end
%画图
figure
hold on;box on;	
plot(X(1,:),X(3,:),'-k');%真实轨迹	
plot(Xekf(1,:),Xekf(3,:),'-r');%扩展Kalman滤波轨迹	
legend(' real trajectory','EKF trajectory');
xlabel('X-axis  X/m');
ylabel('Y-axis Y/m');
figure
hold on ;box on;
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
xlabel('Time /s');
ylabel('Position estimation bias   /m');	
%子函数 求欧氏距离
function d=Dist(X1,X2);
if length(X2)<=2
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);	
else	
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
end
end