In [1]:
# estimating dynamical system states under excessively large measurement noise
using Convex, SCS, PyPlot;
n = 1000;
T = 50;
delt = T/n;
ts = linspace(0,T,n+1);
gamma = 0.05;
# system dynamics
# x(k+1) = A*x(k) + B*w(k)
# y(k) = C*x(k) + v(k)
# where
# x: [ x_position, y_position, x_velocity, y_velocity ]
# y: [ x_position, y_position ]
# w: process noise
# v: measurement noise
A = zeros(4,4);
B = zeros(4,2);
C = zeros(2,4);
A[1,1] = 1;
A[2,2] = 1;
A[1,3] = (1-gamma*delt/2)*delt;
A[2,4] = (1-gamma*delt/2)*delt;
A[3,3] = 1 - gamma*delt;
A[4,4] = 1 - gamma*delt;
B[1,1] = delt^2/2;
B[2,2] = delt^2/2;
B[3,1] = delt;
B[4,2] = delt;
C[1,1] = 1;
C[2,2] = 1;
sigma = 20;
p = 0.20;
x = zeros(4,n+1);
x[:,1] = [0 0 0 0];
y = zeros(2,n);
srand(6);
w = randn(2,n);
v = randn(2,n);
vlarge = rand(n,1);
for t=1:n
if vlarge[t] < 0.2
v[:,t] *= 20;
end
end
for t=1:n
y[:,t] = C*x[:,t] + v[:,t];
x[:,t+1] = A*x[:,t] + B*w[:,t];
end
x_true = x;
w_true = w;
v_true = v;
figure(1);
subplot(2,2,1);
plot(ts,x[1,:]);
xlabel("time");
ylabel("x position");
subplot(2,2,2);
plot(ts,x[2,:]);
xlabel("time");
ylabel("y position");
subplot(2,2,3);
plot(ts,x[3,:]);
xlabel("time");
ylabel("x velocity");
subplot(2,2,4);
plot(ts,x[4,:]);
xlabel("time");
ylabel("y velocity");
figure(2);
subplot(2,2,1);
plot(ts[1:end-1],w_true[1,:]);
xlabel("time");
ylabel("w1");
subplot(2,2,2);
plot(ts[1:end-1],w_true[2,:]);
xlabel("time");
ylabel("w2");
subplot(2,2,3);
plot(ts[1:end-1],v_true[1,:]);
xlabel("time");
ylabel("v1");
subplot(2,2,4);
plot(ts[1:end-1],v_true[2,:]);
xlabel("time");
ylabel("v2");
figure(3);
plot(x[1,:],x[2,:],"o",alpha=0.1);
plot(y[1,:],y[2,:],"ko",alpha=0.1);
title("Trajectory");
legend();
xlim(-10,6);
ylim(-5,35);
In [2]:
# classical kalman filter
x = Variable(4,n+1);
w = Variable(2,n);
v = Variable(2,n);
tau = 0.08;
# 2-norm based loss function
obj = sumsquares(w) + tau*sumsquares(v);
problem = minimize(obj);
for t=1:n
problem.constraints += x[:,t+1] == A*x[:,t] + B*w[:,t] ;
problem.constraints += y[:,t] == C*x[:,t] + v[:,t] ;
end
solve!(problem, SCSSolver(verbose=0))
x_hat2 = evaluate(x);
figure(4);
subplot(2,2,1);
plot(ts,x_true[1,:]);
plot(ts,x_hat2[1,:]);
xlabel("time");
ylabel("x position");
subplot(2,2,2);
plot(ts,x_true[2,:]);
plot(ts,x_hat2[2,:]);
xlabel("time");
ylabel("y position");
subplot(2,2,3);
plot(ts,x_true[3,:]);
plot(ts,x_hat2[3,:]);
xlabel("time");
ylabel("x velocity");
subplot(2,2,4);
plot(ts,x_true[4,:]);
plot(ts,x_hat2[4,:]);
xlabel("time");
ylabel("y velocity");
figure(5);
plot(x_true[1,:],x_true[2,:],"o",alpha=0.1);
plot(x_hat2[1,:],x_hat2[2,:],"o",alpha=0.1);
plot(y[1,:],y[2,:],"ko",alpha=0.1);
title("Trajectory");
xlim(-10,6);
ylim(-5,35);
In [3]:
# huber-norm based estimation
x = Variable(4,n+1);
w = Variable(2,n);
v = Variable(2,n);
tau = 2;
rho = 2;
# huber norm based loss function
obj = sumsquares(w)
obj += sum(tau*huber(norm(v[:,t]),rho) for t in 1:n);
problem = minimize(obj);
for t=1:n
problem.constraints += x[:,t+1] == A*x[:,t] + B*w[:,t] ;
problem.constraints += y[:,t] == C*x[:,t] + v[:,t] ;
end
solve!(problem, SCSSolver(verbose=0))
x_hath = evaluate(x);
figure(6);
subplot(2,2,1);
plot(ts,x_true[1,:]);
plot(ts,x_hat2[1,:]);
plot(ts,x_hath[1,:]);
xlabel("time");
ylabel("x position");
subplot(2,2,2);
plot(ts,x_true[2,:]);
plot(ts,x_hat2[2,:]);
plot(ts,x_hath[2,:]);
xlabel("time");
ylabel("y position");
subplot(2,2,3);
plot(ts,x_true[3,:]);
plot(ts,x_hat2[3,:]);
plot(ts,x_hath[3,:]);
xlabel("time");
ylabel("x velocity");
subplot(2,2,4);
plot(ts,x_true[4,:]);
plot(ts,x_hat2[4,:]);
plot(ts,x_hath[4,:]);
xlabel("time");
ylabel("y velocity");
figure(7);
plot(x_true[1,:],x_true[2,:],"o",alpha=0.1);
plot(x_hat2[1,:],x_hat2[2,:],"o",alpha=0.1);
plot(x_hath[1,:],x_hath[2,:],"o",alpha=0.1);
plot(y[1,:],y[2,:],"ko",alpha=0.1);
title("Trajectory");
xlim(-10,6);
ylim(-5,35);
In [ ]: