扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf)

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

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);

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);
二、扩展Kalman滤波(EKF)算法
EKF算法是一种近似方法，它将非线性模型在状态 估计值附近作泰勒级数展开，并在一阶截断，用得 到的一阶近似项作为原状态方程和测量方程近似表 达形式，从而实现线性化同时假定线性化后的状态 依然服从高斯分布，然后对线性化后的系统采用标 准卡尔曼滤波获得状态估计。采用局部线性化技术， 能得到问题局部最优解，但它能否收敛于全局最优 解，取决于函数的非线性强度以及展开点的选择。

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 滤波函数
EKF与UKF
一、背景
普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得 目标的动态估计，适应于过程和测量都属于线性系统， 且误差符 合高斯分布的系统。 但是实际上很多系统都存在一定的非线性， 表现在过程方程 （状态方程）是非线性的，或者观测与状态之间 的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡 尔曼滤波了。解决的方法是将非线性关系进行线性近似，将其转化 成线性问题。 对于非线性问题线性化常用的两大途径： （1） 将非线性环节线性化，对高阶项采用忽略或逼近措施；（EKF） （2）用采样方法近似非线性分布. ( UKF)

% 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);

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)算法
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;
(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
Qf=[T^2/2 0;0 T;T^2/2 0;0 T];

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) % 量测非线性函数

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

在最近一次状态估计的时刻，对以上两式进
行线性化处理，首先构造如下2个矩阵:

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];

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';
二、扩展Kalman滤波(EKF)算法
Matlab程序：
function test_ekf

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

g = 9.8; % 重力

t = 10; % 仿真时间

Ts = 0.1; % 采样周期

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

% 真实轨迹模拟

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

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

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);
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)算法
将线性化后的状态转移矩阵和观测矩阵代入到标 准卡尔曼滤波框架中，即得到扩展卡尔曼滤波。
因为EKF忽略了非线性函数泰勒展开的高阶项， 仅仅用了一阶项，是非线性函数在局部线性化的结果， 这就给估计带来了很大误差，所以只有当系统的状态 方程和观测方程都接近线性且连续时，EKF的滤波结 果才有可能接近真实值。EKF滤波结果的好坏还与状 态噪声和观测噪声的统计特性有关，在EKF的递推滤 波过程中，状态噪声和观测噪声的协方差矩阵保持不 变，如果这两个噪声协方差矩阵估计的不够准确，那 就容易产生误差累计，导致滤波器发散。EKF的另外 一个缺点是初始状态不太好确定，如果假设的初始状 态和初始协方差误差较大，也容易导致滤波器发散。
2n
Py wic (Yi y)(Yi y)T i0
................(9)
2n
Pxy wic (Yi x)(Yi y)T i0
..............(10)
由于x的均值和方差都精确到二阶，计算得到y 的均值和方差也精确到二阶，比线性化模型精度更 高。在卡尔曼框架内应用UT技术就得到了UKF算法。
%---------------------------------------
function UKFmain
%------------------清屏----------------
close all;clear all;
clc; tic;
global Qf n;
%定义全局变量
%------------------初始化--------------

%%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%%
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;
Y
ekf simulation
520
reaekf 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一样去近似非线性模型，而是对后 验概率密度进行近似来得到次优的滤波算法。

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