Prev: PID controller tuning
Next: unit pulse function
From: Heiko on 7 Apr 2010 05:19
Hi,

i am currently dealing with an implementation of the UKF for Matlab Embedded. Working through several articles i found a general description of my problem, but the proposed solutions did not resolve it.

Here is my problem:
The code shown below shall be implemented as UKF version for dual estimation of states and parameters. The noise is implemented as additive. Three additional parameters (xsi(4) - xsi(6)) have been implemented to model additive errors on the measurement signals.

When i run my simulation with random numbers as excitation, the entries of the covariance matrix P are becoming negative and hence the sigma-point calculation deals with complex numbers.

Do any one of you have comparable issues or is something with my code wrong?

Thank you in advance!!

Code:

function X_UKF = fcn(y, u)

lx = 6; % length x
ly = length(y); % length y

% Standard deviations
stdx = 1e-3;
stdy = 1e-3;

Q = stdx^2*eye(lx); % Process covariance
R = stdy^2*eye(ly); % Measurement covariance

% Parameter settings
alpha = 1e-3;
kappa = 0;
beta = 2;

lambda = alpha^2*(lx+kappa)-lx;
gamma = sqrt(lx+lambda);

xm = zeros(lx,1);
ym = zeros(ly,1);
Pm = zeros(lx);
xsi = zeros(lx,2*lx+1);
XSI = zeros(lx,2*lx+1);
Y = zeros(ly,2*lx+1);
Pxy = zeros(lx,ly);
Pyy = zeros(ly);

Wc = lambda/(lx+lambda)+(1-alpha^2+beta);
Wm = lambda/(lx+lambda);
Wmi = 1/(2*(lx+lambda));
Wci = 1/(2*(lx+lambda));

X_UKF = zeros(lx,1);

persistent x
persistent P

if isempty(x)
x = zeros(lx,1);
x(1) = 0.1;
x(2) = 0.1;
end

if isempty(P)
P = eye(lx);
end

xsi(:,1) = x;

for i = 1:lx
xsi(1:lx,i+1) = x+gamma*sqrt(P(:,i));
xsi(1:lx,i+1+lx) = x-gamma*sqrt(P(:,i));
end

% %Define process functions
% f = [(x(2)-x(1)+u(1))*u(2)+u(2);(x(1)+x(2))*u(1)];
% y = [f(1);f(2)];

% Time Update
for i = 1:2*lx+1
XSI(1:lx,i) = [(xsi(2,i)-xsi(1,i)+u(1)+xsi(4,i))*xsi(3,i)+xsi(3,i);(xsi(1,i)+xsi(2,i))*(u(1)+xsi(4,i));xsi(3,i);xsi(4,i);xsi(5,i);xsi(6,i)];
end

for i = 1:2*lx+1
if i == 1
W = Wm;
else
W = Wmi;
end
xm = xm+W*XSI(:,i);
end

for i = 1:2*lx+1
if i == 1
W = Wc;
else
W = Wci;
end
Pm = Pm+W*(XSI(:,i)-xm)*(XSI(:,i)-xm)'+Q;
end

xsi(:,1) = xm;

for i = 1:lx
xsi(1:lx,i+1) = xm+gamma*sqrt(Pm(:,i));
xsi(1:lx,i+1+lx) = xm-gamma*sqrt(Pm(:,i));
end

for i = 1:2*lx+1
Y(1:ly,i) = [xsi(1,i)+xsi(5,i);xsi(2,i)+xsi(6,i);0;0;0;0];
end

for i = 1:2*lx+1
if i == 1
W = Wm;
else
W = Wmi;
end
ym = ym+W*Y(:,i);
end

% Measurement Update
for i = 1:2*lx+1
if i == 1
W = Wc;
else
W = Wci;
end
Pyy = Pyy + W*(Y(:,i)-ym)*(Y(:,i)-ym)'+R;
Pxy = Pxy + W*(xsi(:,i)-xm)*(Y(:,i)-ym)';
end

K = Pxy*inv(Pyy);
x = xm+K*(y-ym);
P = Pm-K*Pyy*K';

for i = 1:lx
X_UKF(i) = x(i);
end
From: Prashant on 14 Apr 2010 03:25
Hi,
Even I am facing the similar problem when I use random excitation as states in my UKF. To overcome that I tried augmented UKF (assuming the transformation of process and measurement noise will provide more accurate results), but after some iteration P is not positive definite which it should be for correct functioning of UKF.
Tuning of Covariances matrix (P,Q,R) can be done to overcome at least I am able to run through the whole simulation for some values but not getting the desired results. I am still struggling to find a right way to deal with random states.

Thanks

regards
Prashant

"Heiko " <hmikat(a)web.de> wrote in message <hphim8$n0c$1(a)fred.mathworks.com>...
> Hi,
>
> i am currently dealing with an implementation of the UKF for Matlab Embedded. Working through several articles i found a general description of my problem, but the proposed solutions did not resolve it.
>
> Here is my problem:
> The code shown below shall be implemented as UKF version for dual estimation of states and parameters. The noise is implemented as additive. Three additional parameters (xsi(4) - xsi(6)) have been implemented to model additive errors on the measurement signals.
>
> When i run my simulation with random numbers as excitation, the entries of the covariance matrix P are becoming negative and hence the sigma-point calculation deals with complex numbers.
>
> Do any one of you have comparable issues or is something with my code wrong?
>
> Thank you in advance!!
>
> Code:
>
> function X_UKF = fcn(y, u)
>
> lx = 6; % length x
> ly = length(y); % length y
>
> % Standard deviations
> stdx = 1e-3;
> stdy = 1e-3;
>
> Q = stdx^2*eye(lx); % Process covariance
> R = stdy^2*eye(ly); % Measurement covariance
>
> % Parameter settings
> alpha = 1e-3;
> kappa = 0;
> beta = 2;
>
> lambda = alpha^2*(lx+kappa)-lx;
> gamma = sqrt(lx+lambda);
>
> xm = zeros(lx,1);
> ym = zeros(ly,1);
> Pm = zeros(lx);
> xsi = zeros(lx,2*lx+1);
> XSI = zeros(lx,2*lx+1);
> Y = zeros(ly,2*lx+1);
> Pxy = zeros(lx,ly);
> Pyy = zeros(ly);
>
> Wc = lambda/(lx+lambda)+(1-alpha^2+beta);
> Wm = lambda/(lx+lambda);
> Wmi = 1/(2*(lx+lambda));
> Wci = 1/(2*(lx+lambda));
>
> X_UKF = zeros(lx,1);
>
> persistent x
> persistent P
>
> if isempty(x)
> x = zeros(lx,1);
> x(1) = 0.1;
> x(2) = 0.1;
> end
>
> if isempty(P)
> P = eye(lx);
> end
>
> xsi(:,1) = x;
>
> for i = 1:lx
> xsi(1:lx,i+1) = x+gamma*sqrt(P(:,i));
> xsi(1:lx,i+1+lx) = x-gamma*sqrt(P(:,i));
> end
>
> % %Define process functions
> % f = [(x(2)-x(1)+u(1))*u(2)+u(2);(x(1)+x(2))*u(1)];
> % y = [f(1);f(2)];
>
> % Time Update
> for i = 1:2*lx+1
> XSI(1:lx,i) = [(xsi(2,i)-xsi(1,i)+u(1)+xsi(4,i))*xsi(3,i)+xsi(3,i);(xsi(1,i)+xsi(2,i))*(u(1)+xsi(4,i));xsi(3,i);xsi(4,i);xsi(5,i);xsi(6,i)];
> end
>
> for i = 1:2*lx+1
> if i == 1
> W = Wm;
> else
> W = Wmi;
> end
> xm = xm+W*XSI(:,i);
> end
>
> for i = 1:2*lx+1
> if i == 1
> W = Wc;
> else
> W = Wci;
> end
> Pm = Pm+W*(XSI(:,i)-xm)*(XSI(:,i)-xm)'+Q;
> end
>
> xsi(:,1) = xm;
>
> for i = 1:lx
> xsi(1:lx,i+1) = xm+gamma*sqrt(Pm(:,i));
> xsi(1:lx,i+1+lx) = xm-gamma*sqrt(Pm(:,i));
> end
>
> for i = 1:2*lx+1
> Y(1:ly,i) = [xsi(1,i)+xsi(5,i);xsi(2,i)+xsi(6,i);0;0;0;0];
> end
>
> for i = 1:2*lx+1
> if i == 1
> W = Wm;
> else
> W = Wmi;
> end
> ym = ym+W*Y(:,i);
> end
>
> % Measurement Update
> for i = 1:2*lx+1
> if i == 1
> W = Wc;
> else
> W = Wci;
> end
> Pyy = Pyy + W*(Y(:,i)-ym)*(Y(:,i)-ym)'+R;
> Pxy = Pxy + W*(xsi(:,i)-xm)*(Y(:,i)-ym)';
> end
>
> K = Pxy*inv(Pyy);
> x = xm+K*(y-ym);
> P = Pm-K*Pyy*K';
>
> for i = 1:lx
> X_UKF(i) = x(i);
> end
 | 
Pages: 1
Prev: PID controller tuning
Next: unit pulse function