扩展卡尔曼滤波和无迹卡尔曼滤波

二、扩展Kalman滤波(EKF)算法
假定定位跟踪问题的非线性状态方程和测量方程如 下:
X k1 f ( X k ) Wk ...............(1)
Yk h( X k ) Vk ...................(.2)

在最近一次状态估计的时刻，对以上两式进
行线性化处理，首先构造如下2个矩阵:
%---------------------------------------
function UKFmain
%------------------清屏----------------
close all;clear all;
clc; tic;
global Qf n;
%定义全局变量
%------------------初始化--------------

dax = 1.5; day = 1.5; % 系统噪声

X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值

for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
2n
Py wic (Yi y)(Yi y)T i0
................(9)
2n
Pxy wic (Yi x)(Yi y)T i0
..............(10)
由于x的均值和方差都精确到二阶，计算得到y 的均值和方差也精确到二阶，比线性化模型精度更 高。在卡尔曼框架内应用UT技术就得到了UKF算法。

r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);

a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);

Z(k,:) = [r, a];

end
二、扩展Kalman滤波(EKF)算法
figure(1), hold on, plot(Z(:,1).*sin(Z(:,2)), Z(:,1).*cos(Z(:,2)),'*')
...................6( )
三、无迹卡尔曼滤波算法(UKF)
式中: 2 (n k) n , 决定Sigma点的散布程度， 通常取一小的正值,k通常取0; 用来描述x的分 布信息了高斯情况下，的最优值为2);
为(n方 差)P的x 为权矩值阵。平方根第i列；wim为均值的权值，wic

x = fX(1); y = fX(3);

r = sqrt(x^2+y^2);

a = atan(x/y);

hfX = [r; a];

function [Xk, Pk, Kk] = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf 滤波函数

Hk = JacobianH(X(k,:));
fX = fff(X(k,:), kx, ky, g, Ts);

hfX = hhh(fX, Ts);

[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);

X_est(k,:) = Xk';

x1 = x + vx*Ts;

vx1 = vx + (-kx*vx^2)*Ts;

y1 = y + vy*Ts;

vy1 = vy + (ky*vy^2-g)*Ts;

fX = [x1; vx1; y1; vy1];
二、扩展Kalman滤波(EKF)算法
function hfX = hhh(fX, Ts) % 量测非线性函数

% ekf 滤波

Qk = diag([0; dax; 0; day])^2;

Rk = diag([dr; dafa])^2;

Xk = zeros(4,1);

Pk = 100*eye(4);

X_est = X;

for k=1:len

Ft = JacobianF(X(k,:), kx, ky, g);

( 2)计算Sigma点通过非线性函数 f () 的传播结 果:
Yi f ( Xi ), i 0,1,..., 2n .......... ..(7)
从而可知：
三、无迹卡尔曼滤波算法(UKF)
2n
y wimYi .......................................(.8) i0
二、扩展Kalman滤波(EKF)算法
Matlab程序：
function test_ekf

kx = .01; ky = .05; % 阻尼系数

g = 9.8; % 重力

t = 10; % 仿真时间

Ts = 0.1; % 采样周期

len = fix(t/Ts); % 仿真步数

% 真实轨迹模拟
扩展卡尔曼滤波与无迹卡尔曼滤波
EKF与UKF
一、背景
普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得 目标的动态估计，适应于过程和测量都属于线性系统， 且误差符 合高斯分布的系统。 但是实际上很多系统都存在一定的非线性， 表现在过程方程 （状态方程）是非线性的，或者观测与状态之间 的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡 尔曼滤波了。解决的方法是将非线性关系进行线性近似，将其转化 成线性问题。 对于非线性问题线性化常用的两大途径： （1） 将非线性环节线性化，对高阶项采用忽略或逼近措施；（EKF） （2）用采样方法近似非线性分布. ( UKF)
Y
ekf simulation
520
real
measurement
500
ekf estimated
480
460
440
420
400
380
360 0
20 40 60 80 100 120 140 160 180 200 X
图2 仿真结果
三、无迹卡尔曼滤波算法(UKF)
为了改善对非线性问题进行滤波的效果， Julier 等人提出了采用基于unscented变换的UKF方 法UKF不是和EKF一样去近似非线性模型，而是对后 验概率密度进行近似来得到次优的滤波算法。

end

figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on
% figure(2), plot(X(:,2:2:4))

% 构造量测量
mrad = 0.001;

dr = 10; dafa = 10*mrad; % 量测噪声

for k=1:len
stater0=[220; 1;55;-0.5]; %标准系统初值
state0=[200;1.3;50;-0.3]; %测量状态初值
%--------系统滤波初始化
p=[0.005 0 0 0;0 0.005 0 0;
0 0 0.005 0;0 0 0 0.005]; %状态误差协方差初
值
n=4; T=3;
二、扩展Kalman滤波(EKF)算法
EKF算法是一种近似方法，它将非线性模型在状态 估计值附近作泰勒级数展开，并在一阶截断，用得 到的一阶近似项作为原状态方程和测量方程近似表 达形式，从而实现线性化同时假定线性化后的状态 依然服从高斯分布，然后对线性化后的系统采用标 准卡尔曼滤波获得状态估计。采用局部线性化技术， 能得到问题局部最优解，但它能否收敛于全局最优 解，取决于函数的非线性强度以及展开点的选择。
F (k 1k) f ( X k ) X
X X (k k)
.......(3)
H (K ) h( X k ) X
X X (k k 1)
...........(4)
二、扩展Kalman滤波(EKF)算法
将线性化后的状态转移矩阵和观测矩阵代入到标 准卡尔曼滤波框架中，即得到扩展卡尔曼滤波。

x = x + vx*Ts;

vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;

y = y + vy*Ts;
二、扩展Kalman滤波(EKF)算法
vy = vy + (ky*vy^2-g+day*randn(1))*Ts;

X(k,:) = [x, vx, y, vy];

F(3,4) = 1;

F(4,4) = 2*ky*vy;

2) = 1;
二、扩展Kalman滤波(EKF)算法
function H = JacobianH(X) % 量测雅可比函数

x = X(1); y = X(3);

H = zeros(2,4);

r = sqrt(x^2+y^2);
三、无迹卡尔曼滤波算法(UKF)
UKF是用确定的采样来近似状态的后验PDF,可以 有效解决由系统非线性的加剧而引起的滤波发散问 题，但UKF仍是用高斯分布来逼近系统状态的后验概 率密度，所以在系统状态的后验概率密度是非高斯 的情况下，滤波结果将有极大的误差。
三、无迹卡尔曼滤波算法(UKF)
： Matlab程序
Qf=[T^2/2 0;0 T;T^2/2 0;0 T];
UKF算法的核心是UT变换，UT是一种计算非线性 变换中的随机变量的统计特征的新方法，是UKF的基 础。
三、无迹卡尔曼滤波算法(UKF)
假设n维随机向量x : N(x, Px) ,x通过非线性函数y=f(x) 变换后得到n维的随机变量y。通过UT变换可以以较 高的精度和较低的计算复杂度求得y的均值 y 和方 差 Px 。UT的具体过程可描述如下:

%%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%%
function F = JacobianF(X, kx, ky, g) % 系统状态雅可比函数

vx = X(2); vy = X(4);ຫໍສະໝຸດ F = zeros(4,4);

F(1, F(2,2) = -2*kx*vx;
因为EKF忽略了非线性函数泰勒展开的高阶项， 仅仅用了一阶项，是非线性函数在局部线性化的结果， 这就给估计带来了很大误差，所以只有当系统的状态 方程和观测方程都接近线性且连续时，EKF的滤波结 果才有可能接近真实值。EKF滤波结果的好坏还与状 态噪声和观测噪声的统计特性有关，在EKF的递推滤 波过程中，状态噪声和观测噪声的协方差矩阵保持不 变，如果这两个噪声协方差矩阵估计的不够准确，那 就容易产生误差累计，导致滤波器发散。EKF的另外 一个缺点是初始状态不太好确定，如果假设的初始状 态和初始协方差误差较大，也容易导致滤波器发散。

Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;

Pxz = Pkk_1*Hk'; Pzz = Hk*Pxz + Rk;
Pxz*Pzz^-1;

Xk = fXk_1 + Kk*Zk_hfX;
Pk = Pkk_1 - Kk*Pzz*Kk';
Kk =
二、扩展Kalman滤波(EKF)算法

H(1,1) = 1/r; H(1,3) = 1/r;

xy2 = 1+(x/y)^2;

H(2,1) = 1/xy2*1/y; H(2,3) = 1/xy2*x*(-1/y^2);
function fX = fff(X, kx, ky, g, Ts) % 系统状态非线性函数

x = X(1); vx = X(2); y = X(3); vy = X(4);

end
二、扩展Kalman滤波(EKF)算法
figure(1), plot(X_est(:,1),X_est(:,3), '+r')

xlabel('X'); ylabel('Y'); title('ekf simulation');

legend('real', 'measurement', 'ekf estimated');
(1)计算2n+ 1个Sigma点及其权值:
X0

x,

X
i

x

(n 1)Px
X i x (n 1)Px
i 1,2,...,n i n 1,...,2n ......(5)
w0m /(n ) w0c /(n ) (1 2 ) wim wic 1/ 2(n ), i 1,2,...,2n