卡尔曼滤波器

 

目录

1.卡尔曼滤波简介

2.基本模型

　　2.1系统模型

　　2.2观测模型

3.预测与更新

　　3.1预测方程

　　3.2更新方程

4.matlab实现

　　4.1卡尔曼的5个方程说明

　　4.2一个状态估计

　　4.3两个状态估计

　　4.4三个状态估计

2、基本模型

卡尔曼滤波要做的是：

已知：

  1. 系统的初始状态 x0

  2. 每个时间的测量 Z

  3. 系统模型和测量模型

求解：

状态x随着时间变化而产生的值

2.1、系统模型

卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来，符合下式：

系统方程 (1)

  • Fk 是作用在 Xk−1 上的状态变换模型（/矩阵/矢量）。
  • Bk 是作用在控制器向量uk上的输入－控制模型。
  • Wk 是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。

Wk的分布 (2)

2.2、测量模型

时刻k，对真实状态 xk的一个测量zk满足下式:

测量方程 (3)

其中Hk是观测模型,它把真实状态空间映射成观测空间，vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布。

Vk (4)

初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk} 都认为是互相独立的.

 

 

3、预测与更新

3.1、预测方程

预测是这样一个问题：

已知:

  1. 上一个状态的更新值
  2. 上一个状态的更新值和真实值之间的误差

求解：

  1. 这一个状态的预测值
  2. 这一个状态的预测值和真实值之间的误差

过程包括两个方面：

一、由上一个更新值 Xk-1|k-1 预测这一个预测值 Xk|k-1

二、由上一个更新值和真实值之间的误差 Pk-1|k-1 预测下一个预测值和真实值之间的误差 Pk|k-1

具体来说，就是以下两个方程。

预测方程1 (预测状态) (5)

预测方程2 (预测估计协方差矩阵) (6)

这里：

Xk-1|k-1 这种记法代表的是上一次的更新值，后面一个 k-1可以看做 Zk-1， 也就是上一次经过对比Zk-1(实际就是更新)之后所估计出的状态Xk-1。

Xk|k-1 这种记法代表这一次的预测值， 同理于刚才的介绍， 经过上一次Zk-1之后所估计出的状态Xk。

预测公式-预测状态
也就是公式(5)， 可以直接由系统模型导出。

预测公式-协方差矩阵：

P代表着估计误差的协方差，代表着一种 confidence ，比如先验估计误差(预测值与真实值之间误差)的协方差

Pt|t-1的定义

 

3.2、更新方程

更新过程实际上就是一下问题：

已知：

  1. 由上一个更新值得到的当前的预测值。
  2. 当前的观测值
  3. 观测模型

求解：

  1. 融合了预测值和观测的更新值
  2. 由预测值的估计误差得到更新值的估计误差

更新方程如下：

更新方程1

更新方程2

其中K称为kalman增益， 就像一个补偿，决定着预测值应该变化多少幅度，才能变成更新值。
更新方程3

 

4、matlab实现

　4.1、卡尔曼5个公式

4.2、一个状态方程

 1 %本例子从百度文库中得到， 稍加注释
 2 clear
 3 N=200;%取200个数
 4 
 5 %% 生成噪声数据 计算噪声方差
 6 w=randn(1,N); %产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声）
 7 w(1)=0;
 8 Q=var(w); % R、Q分别为过程噪声和测量噪声的协方差(此方程的状态只有一维，方差与协方差相同) 
 9 
10 v=randn(1,N);%测量噪声
11 R=var(v);
12 
13 %% 计算真实状态
14 x_true(1)=0;%状态x_true初始值
15 A=1;%a为状态转移阵，此程序简单起见取1
16 for k=2:N
17     x_true(k)=A*x_true(k-1)+w(k-1);  %系统状态方程，k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量）
18 end
19 
20 
21 %% 由真实状态得到测量数据， 测量数据才是能被用来计算的数据， 其他都是不可见的
22 H=0.2;
23 z=H*x_true+v;%量测方差，c为量测矩阵，同a简化取为一个数
24 
25 
26 %% 开始 预测-更新过程
27 
28 % x_predict: 预测过程得到的x
29 % x_update:更新过程得到的x
30 % P_predict:预测过程得到的P
31 % P_update:更新过程得到的P
32 
33 %初始化误差 和 初始位置
34 x_update(1)=x_true(1);%s(1)表示为初始最优化估计
35 P_update(1)=0;%初始最优化估计协方差
36 
37 for t=2:N
38     %-----1. 预测-----
39     %-----1.1 预测状态-----
40     x_predict(t) = A*x_update(t-1); %没有控制变量
41     %-----1.2 预测误差协方差-----
42     P_predict(t)=A*P_update(t-1)*A'+Q;%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差
43 
44     %-----2. 更新-----
45     %-----2.1 计算卡尔曼增益-----
46     K(t)=H*P_predict(t) / (H*P_predict(t)*H'+R);%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)
47     %-----2.2 更新状态-----
48     x_update(t)=x_predict(t)  +  K(t) * (z(t)-H*x_predict(t));%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)
49     %-----2.3 更新误差协方差-----
50     P_update(t)=P_predict(t) - H*K(t)*P_predict(t);%此式由一步估计的协方差得到此时刻最优化估计的协方差
51 end
52 
53 %% plot
54 %作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态
55 %kalman滤波的作用就是 由绿色的波形得到红色的波形， 使之尽量接近蓝色的真实状态。
56 t=1:N;
57 plot(t,x_update,'r',t,z,'g',t,x_true,'b');

 4.3、两个状态方程

 1 %本例子从百度文库中得到， 稍加注释
 2 clear
 3 N=20;%取200个数
 4 dim=2;
 5 sz=[dim,N];
 6 
 7 x(1)=0.1;
 8 %系统模型
 9 for k=2:20
10    x(k)=x(k-1)+0.1;
11 end
12 %% 生成噪声数据 计算噪声方差
13 % w=sqrt(0.0001)*randn(1,N); %产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声）
14 % %w(1)=0;
15 % Q=var(w); % R、Q分别为过程噪声和测量噪声的协方差(此方程的状态只有一维，方差与协方差相同) 
16 % 
17 % v=sqrt(0.1)*randn(1,N);%测量噪声
18 % R=var(v);
19 
20 q=0.00001;
21 Q=[q/3,q/2;q/2,q];
22 R=0.1;
23 
24 %% 计算真实状态
25 % x_true(1)=0;%状态x_true初始值
26 %F为状态转移阵，此程序简单起见取1
27 % for k=2:N
28 %    x_true(k)=F*x_true(k-1)+w(k-1);    %系统状态方程，k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量）
29 % end
30 F=[1,1;0,1];
31 
32 %% 由真实状态得到测量数据， 测量数据才是能被用来计算的数据， 其他都是不可见的
33 %  H=0.2;
34 %  z=H*x_true+v;%量测方差，c为量测矩阵，同a简化取为一个数
35 H=[1 0];
36 y =[0.3 0.45 0.15 0.48 0.75 0.70 0.95 0.90 0.85 0.80 1.35 1.20 1.15 1.30 1.75 1.40 1.80 2.00 2.10 1.80 ];
37 
38 %% 开始 预测-更新过程
39 
40 % x_predict: 预测过程得到的x
41 % x_update:更新过程得到的x
42 % P_predict:预测过程得到的P
43 % P_update:更新过程得到的P
44 
45 % 对数组进行初始化
46 x_update=zeros(sz); % 对水位的后验估计。即在k时刻，结合浮标当前测量值与k-1时刻先验估计，得到的最终估计值
47 x_predict=zeros(sz); % 水位的先验估计。即在k-1时刻，对k时刻水位做出的估计
48 
49 P_update=zeros(dim,dim,N); % 先验估计的方差
50 P_predict=zeros(dim,dim,N); % 后验估计的方差
51 K=zeros(sz); % 卡尔曼增益，反应了浮标测量结果与过程模型（即当前时刻与下一时刻浮标相同这一模型）的可信程度
52 % I=eye(dim);%单位阵
53 
54 %初始化误差 和 初始位置
55 x_update(:,1)=[0 0]';%s(1)表示为初始最优化估计
56 P_update(:,:,1)=[1000 0;0,1000];%初始最优化估计协方差
57 
58 for t=2:N
59     %-----1. 预测-----
60     %-----1.1 预测状态-----
61     x_predict(:,t) = F*x_update(:,t-1); %没有控制变量
62     %-----1.2 预测误差协方差-----
63     P_predict(:,:,t)=F*P_update(:,:,t-1)*F'+Q;%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差
64 
65     %-----2. 更新-----
66     %-----2.1 计算卡尔曼增益-----
67     K(:,t)=P_predict(:,:,t)*H' / (H*P_predict(:,:,t)*H'+R);%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)
68     %-----2.2 更新状态-----
69     x_update(:,t)=x_predict(:,t)  +  K(:,t) * (y(t)-H*x_predict(:,t));%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)
70     %-----2.3 更新误差协方差-----
71     P_update(:,:,t)=P_predict(:,:,t) - K(:,t)*H*P_predict(:,:,t);%此式由一步估计的协方差得到此时刻最优化估计的协方差
72 end
73 
74 %% plot
75 %作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态
76 %kalman滤波的作用就是 由绿色的波形得到红色的波形， 使之尽量接近蓝色的真实状态。
77 t=1:N;
78 plot(t,y,'r^-',t,x_update(1,t),'g^-',t,x,'b^-');
79 legend('Mesured', 'Estimated', 'Ture Value');
80 xl=xlabel('Time period');
81 yl=ylabel('Level');
82 % set(xl,'fontsize',FontSize);
83 % set(yl,'fontsize',FontSize);
84 set(gca,'color',[0.5,0.5,0.5]);

 

 4.4、三个状态方程

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
%     设置初始化信息  
%  N：设置卡尔曼滤波器追踪点数  
%  r：设置估计变量个数，这里r=3  
%  s：被追踪的火箭的距离，初始值为1000m  
%  v：火箭的速度，初始值为50m/s  
%  a：火箭的加速度，初始值为20m/s2，此时加速度默认为不变  
%  T: 雷达的扫描间隔，此时设为1秒  
%  wt: 系统噪声，方差为20  
%  vt: 量测噪声，方差为16  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
clear all;  
close all;  
N = 100;  
r = 3;  
t = 1:1:N;  
T = 1;  
s = zeros(1,N);  
v = zeros(1,N);  
a = zeros(1,N);  
a(t) = 20;  
s0 = 1000;  
v0 = 50;  
for n = 1:N  
    v(n) = v0 + a(n)*n;  
    s(n) = 1000+v0*n+0.5*a(n)*n*n;  
end  
wt = randn(1,N);  
wt = sqrt(4)*wt./std(wt);  
s = s + wt;  
v = v + wt;  
a(t) = a(t) + wt(t);  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
% 卡尔曼滤波部分，继承之前初始化变量  
% A：转移矩阵  
% H：量测矩阵  
% Qk：系统噪声矩阵  
% Rk：量测噪声矩阵  
% P0：均方误差矩阵初始值  
% Y：火箭的状态矩阵，由k_s，k_v,k_a组成  
% Y0：状态矩阵的初始值  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
Y0 = [900 0 0]';  
Y = [Y0 zeros(r,N-1)];  
A = [1 1 0;0 1 1;0 0 1];  
H = [1 0 0];  
Qk = [0 0 0;0 0 0;0 0 20];  
Rk = 16;  
P0 = [30 0 0;0 20 0;0 0 10];  
P1 = P0;  
P2 = zeros(r,r);  
for k = 1:N  
    Y(:,k) = A*Y(:,k);  
    P2 = A*P1*A'+Qk;  
    Kk = P2*H'*inv(H*P2*H'+Rk);  
    Y(:,k+1) = Y(:,k)+Kk*(s(:,k)-H*Y(:,k));  
    P1 = (eye(r,r)-Kk*H)*P2;  
end  
k_s = Y(1,:);  
k_v = Y(2,:);  
k_a = Y(3,:);  
subplot(3,1,1);  
plot(t,s(t),'-',t,k_s(t),'o');  
title('距离');  
legend('实际值','估计值');  
xlabel('t');  
ylabel('s');  
subplot(3,1,2);  
plot(t,v(t),t,k_v(t),'+');  
title('速度');  
legend('实际值','估计值');  
xlabel('t');  
ylabel('v');  
subplot(3,1,3);  
plot(t,a(t),t,k_a(t),'-x');  
title('加速度');  
legend('实际值','估计值');  
xlabel('t');  
ylabel('a');  
axis([0,N,0,30]);

 

posted on 2014-12-28 12:25  Marty at HDU  阅读(1649)  评论(0)  编辑  收藏  举报

导航