-
Notifications
You must be signed in to change notification settings - Fork 3
/
est_srukf.m
executable file
·129 lines (125 loc) · 4.82 KB
/
est_srukf.m
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
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
function [x,S]=est_srukf(fstate,x,S,hmeas,z,Qs,Rs)
% SR-UKF Square Root Unscented Kalman Filter for nonlinear dynamic systems
% [x, S] = ukf(f,x,S,h,z,Qs,Rs) returns state estimate, x and state covariance, P
% for nonlinear dynamic system (for simplicity, noises are assumed as additive):
% x_k+1 = f(x_k) + w_k
% z_k = h(x_k) + v_k
% where 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: f: function handle for f(x)
% x: "a priori" state estimate
% S: "a priori" estimated the square root of state covariance
% h: fanction handle for h(x)
% z: current measurement
% Qs: process noise standard deviation
% Rs: measurement noise standard deviation
% Output: x: "a posteriori" state estimate
% S: "a posteriori" square root of state covariance
%
% Example:
%{
n=3; %number of state
q=0.1; %std of process
r=0.1; %std of measurement
Qs=q*eye(n); % std matrix of process
Rs=r; % std of measurement
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations
h=@(x)x(1); % measurement equation
s=[0;0;1]; % initial state
x=s+q*randn(3,1); %initial state % initial state with noise
S = eye(n); % initial square root of state covraiance
N=20; % total dynamic steps
xV = zeros(n,N); %estmate % allocate memory
sV = zeros(n,N); %actual
zV = zeros(1,N);
for k=1:N
z = h(s) + r*randn; % measurments
sV(:,k)= s; % save actual state
zV(k) = z; % save measurment
[x, S] = ukf(f,x,S,h,z,Qs,Rs); % ekf
xV(:,k) = x; % save estimate
s = f(s) + q*randn(3,1); % update process
end
for k=1:3 % plot results
subplot(3,1,k)
plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end
%}
% Reference: R. van der Merwe and E. Wan.
% The Square-Root Unscented Kalman Filter for State and Parameter-Estimation, 2001
%
% By Zhe Hu at City University of Hong Kong, 05/01/2017
%
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
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,S,c); %sigma points around x
%[x1,X1,S1,X2]=ut(fstate,X,Wm,Wc,L,Qs); %unscented transformation of process
%Qs = diag([10^-11*ones(1,4) 10^-8 10^-7]);
%Qs = diag([0*ones(1,4) 10^-8 10^-7]);
%Qs = diag([0*ones(1,4) 0 0]);
%Qs = diag([10^-11*ones(1,4) 10^-11 10^-11]);
[x1,X1,S1,X2]=ut(fstate,X,Wm,Wc,L,Qs); %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,S2,Z2]=ut(hmeas,X1,Wm,Wc,m,Rs); %unscented transformation of measurments
P12=X2*diag(Wc)*Z2'; %transformed cross-covariance
K=P12/S2/S2';
x=x1+K*(z-z1); %state update
%S=cholupdate(S1,K*P12,'-'); %covariance update
U = K*S2';
for i = 1:m
S1 = cholupdate(S1, U(:,i), '-');
end
S=S1;
function [y,Y,S,Y1]=ut(f,X,Wm,Wc,n,Rs)
%Unscented Transformation
%Input:
% f: nonlinear map
% X: sigma points
% Wm: weights for mean
% Wc: weights for covraiance
% n: numer of outputs of f
% Rs: additive std
%Output:
% y: transformed mean
% Y: transformed smapling points
% S: transformed square root of 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));
residual=Y1*diag(sqrt(abs(Wc)));
% residual=Y1*diag(sqrt(Wc)); %It is also right(plural)
[~,S]=qr([residual(:,2:L) Rs]',0);
if Wc(1)<0
S=cholupdate(S,residual(:,1),'-');
else
S=cholupdate(S,residual(:,1),'+');
end
% S=cholupdate(S,residual(:,1)); %It is also right(plural)
%P=Y1*diag(Wc)*Y1'+R;
function X=sigmas(x,S,c)
%Sigma points around reference point
%Inputs:
% x: reference point
% S: square root of covariance
% c: coefficient
%Output:
% X: Sigma points
A = c*S';
Y = x(:,ones(1,numel(x)));
X = [x Y+A Y-A];