Intermediate Kalman Filtering theory, Part 5

I think the problem we left off with in part 4 is that we are just asking too much of our sensor. Under these laws of motion, you just can't do it. We could pretty easily imagine a sensor which tracks the azimuth to the satellite as well as the range, but this is hard to do accurately in real life, and besides, isn't that much fun, since the range and azimuth pretty much determine the location without needing a filter.

So, we are going to add a Doppler sensor and measure the range and range-rate, that is, the speed of the target, but only along the line from the satellite to the radar.

As far as the math goes, range rate is just the derivative of range, so let's hit up our favorite new toy and ask it to find a formula for range rate.

derivative of sqrt((x(t)-a)^2+(y(t)-b)^2) with respect to t

I didn't think Alpha would be smart enough to do this, but it is.

((x(t)-a) x'(t)+(y(t)-b) y'(t))/sqrt((a-x(t))^2+(b-y(t))^2)

Everywhere we see x'(t) we think vx, and likewise vy for y'(t). Plus, ρ is hiding in the denominator, so we will use it to make things simpler.

dρ/dt=((rx-mx) vx+(ry-my) vy)/ρ

So, add this to g() as another vector component. g() becomes a two-component vector function of a four-component vector:

ρ(<x>)=sqrt((x.rx-mx)^2+(x.ry-my)^2)
g(<x>)=<ρ,((x.rx-mx) x.vx+(x.ry-my) x.vy)/ρ>

Now we need a new row in [H] also. Since the first component of g() hasn't changed, the first row of [H] is still valid. The second component of g() is a function of all four state vector elements, so all four elements of the second row of [H] are nontrivial. In Alpha, we use x for x.rx, y for x.ry, a for mx, b for my, c for x.vx, and d for x.vy

H[1,0]= ((my-x.ry) (-mx x.vy+my x.vx-x.vx x.ry+x.vy x.rx))/ρ³
H[1,1]= ((mx-x.rx) (mx x.vy-my x.vx+x.vx x.ry-x.vy x.rx))/ρ³
H[1,2]=(x.rx-mx)/ρ
H[1,3]=(x.ry-my)/ρ

Just because the program is easy, we can do it for range and azimuth, also.


%Observation function g(<x>). Takes a state vector, returns an observation
%vector. Primary observation model definition
function g=g_doppler(x_)
  rho=g_radar(x_);
  x=x_(1);
  y=x_(2);
  c=x_(3);
  d=x_(4);
  a=10; %Radar location
  b=0;
  rhodot=((x-a)*c+(y-b)*d)/rho;
  g=[rho;rhodot];
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_doppler(x_)
  rho=g_radar(x_);
  H0=H_radar(x_);
  %We'll follow the names we used in Alpha
  x=x_(1);
  y=x_(2);
  c=x_(3);
  d=x_(4);
  a=10; %Radar location
  b=0;
  num2=(-a*d+b*c-c*y+d*x);
  num1x=(b-y);
  num1y=(a-x);
  H10=(num1x*num2)/rho.^3;
  H11=(num1y*(-num2))/rho.^3;
  H12=(x-a)/rho;
  H13=(y-b)/rho;
  H1=[H10,H11,H12,H13];
  H=[H0;H1];
end

%Observation function g(<x>). Takes a state vector, returns an observation
%vector. Primary observation model definition
function g=g_azimuth(x_)
  rho=g_radar(x_);
  x=x_(1);
  y=x_(2);
  a=10; %Radar location
  b=0;
  az=atan2(y-b,x-a);
  g=[rho;az];
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_azimuth(x_)
  rho=g_radar(x_);
  H0=H_radar(x_);
  %We'll follow the names we used in Alpha
  x=x_(1);
  y=x_(2);
  a=10; %Radar location
  b=0;
  n10=b-y;
  d10=rho^2;
  H10=n10/d10;
  n11=x-a;
  d11=d10;
  H11=n11/d11;
  H12=0;
  H13=0;
  H1=[H10,H11,H12,H13];
  H=[H0;H1];
end