Intermediate Kalman Filtering theory, Part 3

Problem definition
This is the simplest form of the orbit determination problem I learned this filter on back in school. A satellite is in orbit around a point mass. Newton's gravitation applies. The satellite is tracked from a radar at a fixed point above the point mass (or on the surface of a perfectly smooth, homogeneous, radar-transparent planet) in the plane of the satellite orbit. The point mass is so much heavier than the satellite that we may treat the satellite as a test mass. This means that the gravity of the satellite does not significantly move the central mass, and therefore the mass of the satellite cancels right out of the problem.

State Vector
This is like the cart, but now we have two dimensions. It is provable that the trajectory of any test mass around a point mass is confined to a plane containing the point mass, as long as it is only under the influence of gravity, or if all other forces are in the plane also.

So, our state vector has four components, two for position and two for velocity, for a total of four.

<x>=<rx,ry,vx,vy> 

We are using r instead of p for position, mostly for historical reasons. Think of it as the "ray" or "radius" from the center of mass to the satellite.

Physics
This time we use Newton's Universal Law of Gravitation (in title caps, because it is important.)

F=GMm/r²

This is combined with Newton's second law

F=ma

to get

ma=GMm/r²

where

• F is the force on the test mass
• G is the universal gravitational constant
• M is the mass of the central mass
• m is the mass of the test mass
• r=sqrt(rx²+ry²) is the distance from the central mass to the test mass.
• <r>=<rx,ry> is the actual vector from the center to the test mass
• a is the acceleration imposed on the test mass

We see that m appears on both sides, so we will cancel it out and get

a=GM/r²

Also, this is a vector acceleration, directed towards the center of mass, so multiply this by a unit vector pointing from the test mass to the center -<r>/r to get

a=-GM<r>/r³

It is very difficult to measure G by itself, as this involves measuring gravity exerted by objects small enough to fit in a laboratory. It is also very difficult to measure M of a planet by itself, because it's a whole planet, and won't fit on a scale. However, by observing the motions of satellites, it turns out that it is easy to accurately measure the product GM much more accurately than either G or M. So, we call this product μ (Greek small mu) and use it instead.

F(<x>)=a=-μ<r>/r³

And there's our physics function F(). If we want, we can expand it by element

F.rx=0
F.ry=0
F.vx=-μx.rx/sqrt(x.rx²+x.ry²)³
F.vy=-μx.ry/sqrt(x.rx²+x.ry²)³

Observation
Our radar is able to measure the distance (only) from itself to the satellite. Therefore the measurement vector at any time is just this distance, only one component again. The station is located at <m>=<mx,my>, so the distance from the radar to the satellite is

g(<x>)=|<r>-<m>|=sqrt((rx-mx)²+(ry-my)²)

Jacobian matrices
First, [Φ] This one has a lot of blank space because two of its components are simple variables, and the other two are functions of only two elements of the state vector, so right off we have

[Φ]=[0,0,1,0]
       [0,0,0,1]
       [dF.vx/dx.rx,dF.vx/dx.ry,0,0]
       [dF.vy/dx.rx,dF.vy/dx.ry,0,0]

Only six components out of 16 are nonzero, and only four are nontrivial. So, what about the ones that are?

F.vx=-μx.rx/sqrt(x.rx²+x.ry²)³

To the derivinator! derivative of mu*x/(sqrt(x^2+y^2))^3 I don't think that Alpha is smart enough to substitute r, so I just put it in explicitly, and we can sub it back out when we get the result. Alpha (now I want to call it "Ziggy" for some reason) says that the result is

(d)/(dx)((mu x)/sqrt(x^2+y^2)^3) = (mu (y^2-2 x^2))/(x^2+y^2)^(5/2)

or cleaning up and putting back in r

dF.vx/dx.rx= μ (x.ry²-2 x.rx²)/r⁵

We continue to get the other three elements:

dF.vx/dx.ry= -μ(3 x.rx x.ry)/r⁵
dF.vy/dx.rx= -μ(3 x.rx x.ry)/r⁵
dF.vy/dx.ry= μ (x.rx²-2 x.ry²)/r⁵

Now for [H]. This has four elements, one row for the one element of the observation by four columns for the elements of the state, but two of them are zero since the measurement doesn't involve the velocity elements of the state. We will define ρ (Greek small rho) as the measurement itself, so ρ=g(<x>)=sqrt((x.rx-mx)²+(x.ry-my)²)

dg/dx.rx= (x.rx-mx)/ρ
dg/dx.ry= (x.ry-my)/ρ

So we have the two Jacobian matrices,

[Φ]=[0,0,1,0]μ/r⁵
        [0,0,0,1]
        [x.ry²-2 x.rx²,3 x.rx x.ry,0,0]
        [3 x.rx x.ry,x.rx²-2 x.ry²,0,0]

(that extra μ/r⁵ hanging out in front is some common terms factored out) and

[H]=[[(x.rx-mx)/ρ, (x.ry-my)/ρ,0,0]]