卡尔曼滤波器学习笔记（二）

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

经典的卡尔曼滤波只适用于线性且满足高斯分布的系统，但实际工程中并不是这么简单，比如飞行器在水平运动时有可能伴随着自身的自旋，此时的系统并不是线性的，这时就需要应用扩展卡尔曼滤波（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:Nz = h(s) + r*randn;                     sV(:,k)= s;                             %实际状态zV(:,k)  = z;                           %状态测量值[x1,A]=jaccsd(f,x); %计算f的雅可比矩阵，其中x1对应黄金公式line2P=A*P*A'+Q;         %过程方差预测，对应line3[z1,H]=jaccsd(h,x1); %计算h的雅可比矩阵K=P*H'*inv(H*P*H'+R); %卡尔曼增益，对应line4x=x1+K*(z-z1);        %状态EKF估计值，对应line5P=P-K*H*P;            %EKF方差，对应line6xV(:,k) = x;          %saves = f(s) + q*randn(3,1);  %update process
end
for k=1:3FontSize=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:nx1=x;x1(k)=x1(k)+h*i;A(:,k)=imag(fun(x1))/h;
end

运行结果如下：

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

4.下一步

如上讨论，目前两个算法都停留在模拟阶段。接下来准备把EKF应用到自己的飞控板上，移植好之后放代码，看效果！
BTW, 附上本篇代码下载链接，http://download.csdn.net/detail/lizilpl/8643227.

卡尔曼滤波器学习笔记（二）相关推荐

卡尔曼滤波器学习笔记（一）
卡尔曼滤波器的原理及应用最近在学习Probablistic Robotics这本书,获益良多.以前学了概率论和随机过程之后一直觉得这些是虚的,不知道在工程上怎么用,而这本书恰恰就是讲如何把这些概率理 ...
qml学习笔记(二)：可视化元素基类Item详解（上半场anchors等等）
原博主博客地址:http://blog.csdn.net/qq21497936 本文章博客地址:http://blog.csdn.net/qq21497936/article/details/7851 ...
[转载]dorado学习笔记(二)
原文地址:dorado学习笔记(二)作者:傻掛 ·isFirst, isLast在什么情况下使用?在遍历dataset的时候会用到 ·dorado执行的顺序,首先由jsp发送请求,调用相关的ViewM ...
PyTorch学习笔记(二)——回归
PyTorch学习笔记(二)--回归本文主要是用PyTorch来实现一个简单的回归任务. 编辑器:spyder 1.引入相应的包及生成伪数据 import torch import torch.nn ...
tensorflow学习笔记二——建立一个简单的神经网络拟合二次函数
tensorflow学习笔记二--建立一个简单的神经网络 2016-09-23 16:04 2973人阅读评论(2) 收藏举报分类: tensorflow(4) 目录(?)[+] 本笔记目的 ...
Scapy学习笔记二
Scapy学习笔记二 Scapy Sniffer的用法: http://blog.csdn.net/qwertyupoiuytr/article/details/54670489 Scapy Snif ...
Ethernet/IP 学习笔记二
Ethernet/IP 学习笔记二原文链接:http://wiki.mbalib.com/wiki/Ethernet/IP 1.通信模式不同于源/目的通信模式,EtherNet/IP 采用生产/消 ...
Java学习笔记二：数据类型
Java学习笔记二:数据类型 1. 整型:没有小数部分,允许为负数,Java整型分4种:int short long byte 1.1 Int最为常用,一个Int类型变量在内存中占用4个字节,取值范围 ...
吴恩达《机器学习》学习笔记二——单变量线性回归
吴恩达<机器学习>学习笔记二--单变量线性回归一. 模型描述二. 代价函数 1.代价函数和目标函数的引出 2.代价函数的理解(单变量) 3.代价函数的理解(两个参数) 三. 梯度下降- ...