Unscented kalman Filtering 无损卡尔曼滤波的matlab程序

本来想写下原理的,CSDN的博客貌似不可以编辑公式,那就算了,直接放程序好了。

这个程序大部分是从前人那里看来的,向他们表示感谢,它帮助我完成了一个课程论文的仿真。我很喜欢原作者简洁有效的编程风格。这几个程序对于学习最优估计课程的人来说是很需要的。

构建Sigma点的函数

function X = sigmas(x,P,c)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sigma points around reference point
% 构造2n+1个sigma点
% Inputs:
% x: reference point
% P: covariance
% c: coefficient
% Output:
% X: Sigma points
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

A = c*chol(P)';
Y = x(:,ones(1,numel(x)));
X = [x Y+A Y-A];
UT变换函数

function [y,Y,P,Y1] = ut(f,X,Wm,Wc,n,R)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Unscented Transformation
% UT转换函数
% Input:
% f: nonlinear map
% X: sigma points
% Wm: weights for mean
% Wc: weights for covraiance
% n: numer of outputs of f
% R: additive covariance
% Output:
% y: transformed mean
% Y: transformed smapling points
% P: transformed covariance
% Y1: transformed deviations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L = size(X,2);
y = zeros(n,1);
Y = zeros(n,L);
for k=1:L
    Y(:,k) = f(X(:,k));
    y = y+Wm(k)*Y(:,k);
end
Y1 = Y-y(:,ones(1,L));
P = Y1*diag(Wc)*Y1'+R;
Unscented kalman Filtering 函数

function [x,P] = ukf(fstate, x, P, hmeas, z, Q, R)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% UKF Unscented Kalman Filter for nonlinear dynamic systems
% 无损卡尔曼滤波(Unscented Kalman Filter)函数,适用于动态非线性系统
% for nonlinear dynamic system (noises are assumed as additive):
%   x_k+1 = f(x_k) + w_k
%   z_k = h(x_k) + v_k
% w ~ N(0,Q) meaning w is gaussian noise with covariance Q
% v ~ N(0,R) meaning v is gaussian noise with covariance R
% =============================参数说明=================================
% Inputs: 
% fstate  -[function]: 状态方程f(x)
%     x   -     [vec]: 状态先验估计 "a priori" state estimate
%     P   -     [mat]: 方差先验估计 "a priori" estimated state covariance
% hmeas   -[function]: 量测方程h(x)
%     z   -     [vec]: 量测数据     current measurement
%     Q   -     [mat]: 状态方程噪声w(t) process noise covariance
%     R   -     [mat]: 量测方程噪声v(t) measurement noise covariance
% Output:
%     x   -     [mat]: 状态后验估计 "a posteriori" state estimate
%     P   -     [mat]: 方差后验估计 "a posteriori" state covariance
% =====================================================================
% By Yi Cao at Cranfield University, 04/01/2008
% Modified by JD Liu 2010-4-20
% Modified by zhangwenyu, 12/23/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<7
    error('Not enough inputarguments!');
end

% 初始化,为了简化函数,求lamda的过程被默认
L = numel(x);                                 %numer of states
m = numel(z);                                 %numer of measurements
alpha = 1e-3;                                 %default, tunable
ki = 0;                                       %default, tunable
beta = 2;                                     %default, tunable

% UT转换部分
lambda = alpha^2*(L+ki)-L;                    %scaling factor
c = L+lambda;                                 %scaling factor
Wm = [lambda/c 0.5/c+zeros(1,2*L)];           %weights for means
Wc = Wm;
Wc(1) = Wc(1)+(1-alpha^2+beta);               %weights for covariance
c = sqrt(c);
X = sigmas(x,P,c);                            %sigma points around x
[x1,X1,P1,X2] = ut(fstate,X,Wm,Wc,L,Q);       %unscented transformation of process
% X1=sigmas(x1,P1,c);                         %sigma points around x1
% X2=X1-x1(:,ones(1,size(X1,2)));             %deviation of X1
[z1,Z1,P2,Z2] = ut(hmeas,X1,Wm,Wc,m,R);       %unscented transformation of measurments

% 滤波部分
P12 = X2*diag(Wc)*Z2';                        %transformed cross-covariance
K = P12*inv(P2);
x = x1+K*(z-z1);                              %state update
P = P1-K*P12';                                %covariance update

附带一个原本程序里面使用的例子,可以学习怎么使用这几个函数。当然,如果你懂得无损卡尔曼滤波的过程和用法,这些或许是不需要的。
%n=3; %number of state
clc;
clear;
n=6;
t=0.2;
q=0.1; %std of process
r=0.7; %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2; % covariance of measurement
%f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations
f=@(x)[x(1)+t*x(3);x(2)+t*x(4);x(3)+t*x(5);x(4)+t*x(6);x(5);x(6)]; % nonlinear state equations
h=@(x)[sqrt(x(1)+1);0.8*x(2)+0.3*x(1);x(3);x(4);x(5);x(6)];
% measurement equation
%s=[0;0;1]; % initial state
s=[0.3;0.2;1;2;2;-1];
x=s+q*randn(n,1); %initial state % initial state with noise
P = eye(n); % initial state covraiance
N=20; % total dynamic steps
xV = zeros(n,N); %estmate % allocate memory
sV = zeros(n,N); %actual
zV = zeros(n,N);
for k=1:N
    z = h(s) + r*randn; % measurments
    sV(:,k)= s; % save actual state
    zV(:,k) = z; % save measurment
    [x, P] = ukf(f,x,P,h,z,Q,R); % ukf
    xV(:,k) = x; % save estimate
    s = f(s) + q*randn(n,1); % update process
end
for k=1:4 % plot results
    subplot(4,1,k)
    plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--',1:N,zV(k,:),'*')
end
相信有了它们,无论你是做科研仿真,还是一般的用途,都足以用于参考或使用。
相关文章
相关标签/搜索