We can hang this model on the same scaffold we built last time. Mostly we just change the model functions, but we are also going to change the simulator at the top, since this model is more complicated than last time.
We are going to orbit planet B612, a perfectly spherical, homogeneous, radar-transparent planet which has a radius of 10m but sufficient gravity to live on conveniently. In units of meters and seconds, it has a μ of 1000, which means that a ball thrown at 10m/s at the surface will stay in a circular orbit right at the surface, and circle the planet in 6.283 seconds. The planet has a surface gravity of about 1g, exactly 10m/s^2.
As an aside, B612 is one weird planet. It needs to weigh 15 billion tons to exert this much gravity, and has a density of 3.58 million kg/m^3, which about as dense as white dwarf degenerate matter. And yet it's radar-transparent...
Our program is pretty heavily modified here, so let's look at it fresh again. This first part is the driver, and as we can see, most of the code has been farmed out to various subfunctions.
function nonlinear_B612
%Simulated actual trajectory. Unperturbed orbit around B612 at slightly
%higher than circlular velocity.
t=0:0.1:10; %Timestamp of each measurement
xt0=[11;0;0;10]; %Position <11,0> meters from center, velocity <0,10>
xt=trueState(t,xt0,@F_B612);
R=0.5^2; %Measurement noise covariance, 50cm on position
Q=diag([0,0,0.01,0.01]); %Process noise covariance, 0 position and 10cm/s^2 on velocity
zt=trueObs(xt,@g_B612,0);
z=trueObs(xt,@g_B612,R);
xh_0=[12;0;0;9]; %Initial state vector guess, slightly wrong
P_0=diag([1,1,1,1]); %Initial state vector uncertainty, 1m in position and 1m/s in velocity
[xh,P]=ekf_loop(xh_0,P_0,z,t,@F_B612,@g_B612,@Phi_B612,@H_B612,Q,R);
%Plot some interesting results
figure(1)
plot(10*cos([0:0.01:2*pi]),10*sin(0:0.01:2*pi),'k-',...
xt(1,:),xt(2,:),'r-', ...
xh(1,:),xh(2,:),'b-');
legend('Planet B612 surface','True position','Estimated position');
axis equal;
%measurement residual plot
zh=trueObs(xh,@g_B612,0);
figure(2)
plot(t,zt,'r-',t,zh,'b-');
legend('Actual radar distance','Radar distance from estimate');
end
In this first bit, we call a function called trueState() which we pass in the initial conditions, the timestamps we care about, and the physics function. It then does all the integrating and returns a list of true state vectors at all the requested times.
This @f bit is incredibly important. We are passing a function name as a parameter. trueState() will then call that function by name. In this way, we can use the same code to call our own model functions back, so the same framework can support any physics model. The same thing can be done in C with function pointers, in Java with interfaces, and in most truly useful languages in general.
function xt=trueState(t,xt0,F)
xt=zeros(numel(xt0),numel(t)); %True state vector (to be calculated)
xt(:,1)=xt0;
%Do a numerical integrator to get the positions
nstep=100;
for i=1:numel(t)-1
I=i+1;
xt_im1=xt(:,I-1); %Previous true state
xt_i=xt_im1; %Will contain current true state
dt_major=t(I)-t(I-1);
dt=dt_major/nstep; %Do 100 steps between measurements
for j=1:nstep
%Just use our physics function - that's what it is there for.
xt_i=xt_i+dt*F(xt_i);
end
xt(:,I)=xt_i;
end
end
Next, we call trueObs(), which takes a list of state vectors and the observation function, and produces a list of observations, with noise sprinkled on it if requested. In this case, we call it twice, once with noise and once without, so we can compare the Truth to our estimate.
function z=trueObs(xt,g,R)
%Size the problem
xt0=xt(:,1);
s=size(xt)
z=zeros(numel(g(xt0)),s(2));
%Clean observations
for i=1:numel(z)
z(:,i)=g(xt(:,i));
end
%We want random noise, but the *same* random noise each time we run
randn('seed',3217);
pn=randn(size(z))*sqrt(R); %Measurement noise
z=z+pn; %Noisy Observations
end
Next, we set up the initial guess and noise estimates, then call ekf_loop() to actually run the Extended Kalman filter. It takes a lot of parameters, but none of them are complicated. The hardest part is just passing them in the correct order.
function [xh,P]=ekf_loop(xh_0,P_0,z,t,F,g,Phi,H,Q,R)
s=size(z);
n=s(2); %Number of observations
xh=zeros(numel(xh_0),n); %History of all estimates (starts as initial estimate)
xh(:,1)=xh_0;
P=zeros(numel(xh_0),numel(xh_0),n); %History of all estimate covariances
P(:,:,1)=P_0;
for i=1:n-1
%Stupid matlab, not using zero-based indexing. Our i here is zero based,
%but we will make an I which is one-based, and use it to index the
%history arrays.
I=i+1;
%Pull previous estimate from histories
xh_im1=xh(:,I-1); %Previous extimate <x^_{i-1}>
P_im1=P(:,:,I-1); %Previous covariance <P^_{i-1}>
dt_major=t(I)-t(I-1);
[xh_i,P_i]=ekf_step(xh_im1,P_im1,z(I),dt_major,@F_B612,@g_B612,@Phi_B612,@H_B612,Q,R);
%Record the current estimates
xh(:,I)=xh_i;
P(:,:,I)=P_i;
end
end
The function ekf_loop takes the following inputs:
- xh_0 - initial guess of the state vector
- P_0 - initial guess of the estimate covariance
- z - list of measurement vectors
- t - array of times, same length as number of measurements
- F - Name of physics function
- g - Name of observation function
- Phi - Name of Physics matrix function
- H - Name of observation matrix function
- Q - Process noise covariance matrix, square n x n where n is number of elements of state vector
- R - Measurement noise covariance matrix, square m x m where m is number of elements of measurement vector
It produces the following outputs:
- xh - list of estimated states
- P - list of estimate covariance matrices
If you have a whole bunch of measurements in a block that you want to do, this is the way to go. This function calls ekf_step() to actually run the equations we have derived. If you get your measurements one at a time and don't care so much about history, perhaps because you are running a robot control loop, you can call ekf_step() directly.
function [xh_i,P_i]=ekf_step(xh_im1,P_im1,z_i,dt_major,F,g,Phi,H,Q,R)
%One numerical integrator for both 1A and 1B, so that 1B can use the
%intermediate results of 1A.
xh_im=xh_im1; %To become updated state estimate <x^_i->
A=eye(size(P_im1)); %The thing we integrate in 1b, will be come [A]
nstep=100;
dt=dt_major/nstep;
for j=1:nstep
%1a. Use numerical integration to update state vector through time from
%<x^_{i-1}> to <x^_i->
xh_im=xh_im+dt*F(xh_im);
%1b. Use numerical integration to update [A]
A=A+dt*(Phi(xh_im)*A);
end
%1c, state deviation estimate
Xh_im=zeros(size(xh_im));
%2. Projected covariance estimate
P_im=A*P_im1*A'+Q;
%3a. Linearized observation matrix
H_i=H(xh_im);
%3b. Kalman gain
K=P_im*H_i'*inv(H_i*P_im*H_i'+R);
%4a. Measurement deviation
Z_i=z_i-g(xh_im);
%4b. Measurement update of state deviation
Xh_i=Xh_im+K*(Z_i-H_i*Xh_im);
%4c. Update of reference state vector
xh_i=xh_im+Xh_i;
%5. Measurement update of state covariance
P_i=(eye(size(P_im))-K*H_i)*P_im;
end
Its inputs are:
- xh_im1 - Previous estimate of the state vector
- P_im1 - Previous estimate covariance
- z_i - current measurement vector
- dt_major - time between last and current measurement
- F - Name of physics function
- g - Name of observation function
- Phi - Name of Physics matrix function
- H - Name of observation matrix function
- Q - Process noise covariance matrix, square n x n where n is number of elements of state vector
- R - Measurement noise covariance matrix, square m x m where m is number of elements of measurement vector
- xh_i - New estimated state vector
- P_i - New estimate covariance matrix
%Physics function F(<x>). Takes a state vector, returns the derivative of
%the state vector. Primary physics model definition
function F=F_B612(x)
rx=x(1);
ry=x(2);
vx=x(3);
vy=x(4);
r=[rx;ry];
r3=sum(r.^2)^1.5;
mu=1000;
a=-mu*r/r3;
F=[vx;vy;a(1);a(2)];
end
%Physics matrix [Phi(<x>)]. Takes the state vector and returns the jacobian
%matrix. Secondary physics model definition, as it is derived from F() (on
%paper).
function Phi=Phi_B612(x)
mu=1000;
rx=x(1);
ry=x(2);
r=[rx;ry];
r5=sum(r.^2)^2.5;
Phi=[0,0,1,0;
0,0,0,1;
[ry^2-2*rx^2,3*rx*ry,0,0]*mu/r5;
[3*rx*ry,rx^2-2*ry^2,0,0]*mu/r5];
end
%Observation function g(<x>). Takes a state vector, returns an observation
%vector. Primary observation model definition
function g=g_B612(x)
rx=x(1);
ry=x(2);
r=[rx;ry];
mx=10; %Radar location
my=0;
g=sqrt((rx-mx)^2+(ry-my)^2);
end
%Observation matrix [H(<x>)]. Takes the state vector and returns the jacobian
%matrix. Secondary observation model definition, as it is derived from g() (on
%paper).
function H=H_B612(x)
rho=g_B612(x);
rx=x(1);
ry=x(2);
r=[rx;ry];
mx=10; %Radar location
my=0;
H=[(rx-mx)/rho,(ry-my)/rho,0,0];
end
Let's see how we did:
Edit: There was a bug, in the Phi function. I had factored some stuff out, the mu/r^5 term, but when I realized we had the 1 terms in the first couple of rows, those were being multiplied by mu/r^5 also, which is not correct. With the correct Phi, we get the following:
