## Thursday, March 31, 2011

### Intermdiate Kalman Filtering Theory, Part 4

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
and its outputs are:

• xh_i - New estimated state vector
• P_i - New estimate covariance matrix
Finally, the model functions for B612:

 %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:

Hmm, not so good. We have diverged from reality. Exercise for the reader: Why? Is our initial guess too far off? Is it too noisy? Is there a bug? Can we just not do the estimation with this kind of measurement? I have some ideas, what about yours? Try messing with the initial condition and noise variables and see if you can get it to converge. I have my own ideas, which I will share next post.

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.