卡尔曼/扩展卡尔曼滤波器的原理及应用

卡尔曼滤波器的原理及应用

应用前提
算法详细介绍
应用举例
下一步

原文地址:http://blog.csdn.net/lizilpl/article/details/45289541

1.应用前提

要应用kalman Filter,首先要有三个前提假设：

当前状态的概率分布必须是上一状态和将要执行的控制量的线性函数，再叠加一个高斯噪声。表达式如下：



其中

和

是状态变量，如果系统有多个自由度的话，它们表示状态向量。这样的话根据高斯分布


可以得到状态转移概率





其中

表示上一状态的均值，

表示方差。

对状态的测量必须是状态的线性函数叠加高斯噪声





其含义与上类似，不再赘述。

初始状态分布为高斯分布

2.算法详细介绍

Kalman Filter五条黄金公式 ：



这五条公式基本上就是Kalman Filter的主要内容了， 它的本质就是通过预测结合测量来估计当前系统的状态。举个lizi，假如我们要估计一架飞行器的姿态，可以通过IMU来实时测量，但是测量值有一定的风险是不准确的，所以并不能完全依赖传感器。任何一个满足物理规律的系统应当是连续的，所以我们还可以通过上一状态来预测当前状态。Kalman Filter正是结合这两条进行状态估计，到底是相信哪一个多一点，还要根据Kt来决定，我们定义Kt为卡尔曼增益，它是根据
测量和预测的协方差来计算的。
下面逐条解释：

line 2： 首先通过上一状态最优值和将要施加的控制量来预测当前状态，由假设一可以得到：



因为我们只是求均值，而高斯噪声均值为0，所以可省去最后一项。

line 3： 除了预测均值之外，我们还需要预测值的协方差来计算Kalman增益。



根据假设2，这条公式可以很容易得到。

line 4：准备工作完成之后，需要根据预测值的协方差

，测量值和状态
的比例系数，测量值的协方差来计算Kalman增益。



具体证明需要用到假设中的高斯分布公式，因为我们只是应用，所以就不在blog中讨论啦，感兴趣的小伙伴可以看书中3.2.4节 Mathematical Derivation of the KF，里面讲的很详细，分享一下下载链接

http://download.csdn.net/detail/lizilpl/8632071。

line 5：这一行可以说是Kalman Filter 的精华了，现在我们有了对状态的预测值和协方差，同时也收集到了对状态的测量值。这时就可以通过kalman增益来计算状态估计值了。



增益越大，表明我们越相信测量值。

line 6： 根据 line3 ，预测当前状态需要用到上一状态的协方差，所以我们还需要计算当前状态的协方差用于下一次迭代。它同样要根据Kalman增益来计算：



相信到这里，大家应该对kalman Filter的原理有了一个大致的了解，算法中，从初始状态开始，不断计算当前状态的均值和方差来迭代，直至系统结束。

3.应用实例

如下程序引用自百度百科

http://baike.baidu.com/link?url=g11J2Ab9SHiYaGB34hl86UxEMnaJyXwi_I5SrTrzKDYEMSynK1zO1Is0oXVZkR1yNKbtudaGws8j7NAdkLuV8q

程序描述了如下一个系统：

房间温度为24度
房间内连续两个时刻温度差值的标准差为0.02度
温度计的测量值误差的标准差为0.5度
对温度的初始估计值为23.5度，误差的方差为1
对整个系统的控制量为0

现在需要利用Kalman Filter来估计房间的实时温度

MatLab仿真的代码如下：% Kalman filter example of temperature measurement in Matlab% This M code is modified from Xuchen Yao's matlab on 2013/4/18%房间当前温度真实值为24度，认为下一时刻与当前时刻温度相同，误差为0.02度（即认为连续的两个时刻最多变化0.02度）。%温度计的测量误差为0.5度。%开始时，房间温度的估计为23.5度，误差为1度。% Kalman filter example demo in Matlab% This M code is modified from Andrew D. Straw's Python% implementation of Kalman filter algorithm.clear all;close all;% intial parameters% 初始化参数
n_iter = 100; %计算连续n_iter个时刻
sz = [n_iter, 1];
x = 24; % 温度的真实值
Q = 4e-4; % 对温度预测值的方差
R = 0.25; % 测量方差，反应温度计的测量精度
T_start = 23.5; %温度初始估计值
delta_start = 1; %温度初始估计方差
z = x + sqrt(R)*randn(sz);
% z是温度计的测量结果，在真实值的基础上加上了方差为0.25的高斯噪声。
% 初始化数组
state_kalman=zeros(sz);
% 对温度的估计值。即在k时刻，结合温度计当前测量值与k-1时刻预测值，得到的最终估计值
variance_kalman=zeros(sz); % 估计值的方差
state_pre=zeros(sz); % 对温度的预测
variance_pre=zeros(sz); % 预测值的方差
K=zeros(sz); % 卡尔曼增益
state_kalman(1) = T_start; %温度估计值初始化
variance_kalman(1) =delta_start; %估计值方差初始化
%
%开始迭代计算
for k = 2:n_iter
state_pre(k) = state_kalman(k-1);
%用上一时刻的最优估计值来作为对当前时刻的温度的预测
variance_pre(k) = variance_kalman(k-1)+Q;
%预测的方差为上一时刻温度最优估计值的方差与高斯噪声方差之和
%
%计算卡尔曼增益
K(k) = variance_pre(k)/( variance_pre(k)+R );
%
%结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。由于是直接测量，故C为1.
state_kalman(k) = state_pre(k)+K(k)*(z(k)-state_pre(k));
variance_kalman(k) = (1-K(k))*variance_pre(k);
%计算最终估计值的方差用于下一次迭代
end
%绘图相关。。。。。
FontSize=14;
LineWidth=3;
figure();
plot(z,'k+'); %画出温度计的测量值
hold on;
plot(state_kalman,'b-','LineWidth',LineWidth) %画出最优估计值
hold on;
plot(x*ones(sz),'g-','LineWidth',LineWidth); %画出真实值
legend('温度测量值', 'Kalman估计值', '真实值');
xl=xlabel('时间(分钟)');
yl=ylabel('温度');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
hold off;
set(gca,'FontSize',FontSize);
figure();
valid_iter = [2:n_iter]; % variance_pre not valid at step 1
plot(valid_iter,variance_kalman([valid_iter]),'LineWidth',LineWidth); %画出最优估计值的方差
legend('Kalman估计的误差估计');
xl=xlabel('时间(分钟)');
yl=ylabel('℃^2');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
set(gca,'FontSize',FontSize);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
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
[/code]
“`

运行结果如下：





由结果可以看出来，虽然测量值噪声很大，但Kalman Filter的预测值还是逐渐逼近了真实温度，并且估计值的方差逐渐收敛！

4.下一步

如上讨论，经典的卡尔曼滤波只适用于线性且满足高斯分布的系统，但实际工程中并不是这么简单，比如飞行器在水平运动时有可能伴随着自身的自旋，这时就需要应用扩展卡尔曼滤波（EKF）来解决这种情况。等我研究完之后和大家分享。

扩展卡尔曼滤波器的原理及应用

经典的卡尔曼滤波只适用于线性且满足高斯分布的系统，但实际工程中并不是这么简单，比如飞行器在水平运动时有可能伴随着自身的自旋，此时的系统并不是线性的，这时就需要应用扩展卡尔曼滤波（EKF）来解决这种情况

应用前提
EKF算法详细介绍
应用举例
下一步

1.应用前提

与kalman Filter只能应用于线性系统不同,Extended Kalman Filter 可以用于非线性系统中。：

当前状态的概率分布是关于上一状态和将要执行的控制量的二元函数，再叠加一个高斯噪声，测量值同样是关于当前状态的函数叠加高斯噪声。具体表达式如下：




和

可以是非线性的函数。

为了用经典卡尔曼滤波器的思想来解决非线性系统中的状态估计问题，首先要做的就是把

和

用泰勒级数展开，将其线性化，只取一次项为一阶EKF滤波。具体如下：






在上一状态估计的最优值处取一阶导数，

在当前时刻预测值处取一阶导数，得到G和H分别相当于Kalman
Filter中的A和C。

2.EKF算法详细介绍

Extended Kalman Filter五条黄金公式 ：



将其线性化之后，EFK与FK就非常类似了，所以我就不再赘述，可参考上一篇blog：卡尔曼滤波器的原理及应用 http://blog.csdn.net/lizilpl/article/details/45268471

里面对这五条公式有较为详细的介绍。

3.应用实例

下面这个小程序模拟了三维状态非线性系统的EKF算法。

% author :  Perry.Li  @USTC
% function: simulating the process of EKF
% date:     04/28/2015
%
N = 50;         %计算连续N个时刻
n=3;            %状态维度
q=0.1;          %过程标准差
r=0.2;          %测量标准差
Q=q^2*eye(n);   %过程方差
R=r^2;          %测量值的方差
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  %状态方程
h=@(x)[x(1);x(2);x(3)];                   %测量方程
s=[0;0;1];                                %初始状态
%初始化状态
x=s+q*randn(3,1);
P = eye(n);
xV = zeros(n,N);
sV = zeros(n,N);
zV = zeros(n,N);
for k=1:N
z = h(s) + r*randn;
sV(:,k)= s;                             %实际状态
zV(:,k)  = z;                           %状态测量值
[x1,A]=jaccsd(f,x); %计算f的雅可比矩阵，其中x1对应黄金公式line2
P=A*P*A'+Q;         %过程方差预测，对应line3
[z1,H]=jaccsd(h,x1); %计算h的雅可比矩阵
K=P*H'*inv(H*P*H'+R); %卡尔曼增益，对应line4
x=x1+K*(z-z1);        %状态EKF估计值，对应line5
P=P-K*H*P;            %EKF方差，对应line6
xV(:,k) = x;          %save
s = f(s) + q*randn(3,1);  %update process
end
for k=1:3
FontSize=14;
LineWidth=1;
figure();
plot(sV(k,:),'g-'); %画出真实值
hold on;
plot(xV(k,:),'b-','LineWidth',LineWidth) %画出最优估计值
hold on;
plot(zV(k,:),'k+'); %画出状态测量值
hold on;
legend('真实状态', 'EKF最优估计估计值','状态测量值');
xl=xlabel('时间(分钟)');
t=['状态 ',num2str(k)] ;
yl=ylabel(t);
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
hold off;
set(gca,'FontSize',FontSize);
end

用到的Function为计算 jacobi matrix，如下：

function [z,A]=jaccsd(fun,x)
% JACCSD Jacobian through complex step differentiation
% [z J] = jaccsd(f,x)
% z = f(x)
% J = f'(x)
%
z=fun(x);
n=numel(x);
m=numel(z);
A=zeros(m,n);
h=n*eps;
for k=1:n
x1=x;
x1(k)=x1(k)+h*i;
A(:,k)=imag(fun(x1))/h;
end

运行结果如下：







三个图为三维状态的真实值，测量值，EKF估计值对比图，效果拔群！