**Worked example**

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:

This is better, in that the path is not wandering off into deep space. However, it's still not great. What is amazing is how well this meandering path fits the actual data:

What's going on here is that you just can't completely observe the orbit based on only range measurements. That's the answer I expected above.