Radiation-pressure propulsion for nano-spacecraft

Abstract: The Breakthrough Starshot project proposes using radiation-pressure propulsion to send multiple spacecraft in the gram-range mass to interstellar targets. There are numerous engineering challenges to accomplishing this mission, but their project discusses speeds and accelerations previously only discussed for cannon projectiles and particle physics experiments. This article examines the problem from first principles to see if the project is even orders-of-magnitude possible. A preliminary check on their numbers seems to confirm that the project is possible in principle.

 Radiation pressure has been hypothesized since the early days of special relativity and quantum mechanics. It is a simple consequence of the mass-energy equivalence and the photon nature of light. Quantum mechanics states that light is made of photons, and that each photon is a discrete bundle of energy. The energy \(E_{pho}\) carried by each photon is proportional to its frequency \(\nu=c/\lambda\), and the proportionality constant is Planck's constant \(h\).

\[E_{pho}=h\nu=\frac{hc}{\lambda}\]

Everyone knows the famous mass-energy equivalence equation \(E=mc^2\). However, that is only a special case of the momentum equation:

\[E=\sqrt{p^2c^2+m_0^2c^4}\]

We can solve this for the momentum of a photon, considering that its rest mass is zero.

\[\begin{eqnarray}E_{pho} & = & \sqrt{p_{pho}^2c^2+0^2c^4}\\
& = & \sqrt{p_{pho}^2c^2} \\
& = & pc\\
\frac{hc}{\lambda}&=&p_{pho}c\\
\frac{h}{\lambda}&=&p_{pho}\end{eqnarray}\]

Note that momentum is a vector quantity, but this just deals with the magnitude. The direction of the momentum vector is the direction the photon is traveling.

Now, we know the energy and momentum of each photon, so we know that if we throw so many joules of photons at a target, it will transfer so much momentum. The irradiance is defined as the power \(P\) (energy \(E\) per unit time \(t\)) of the light hitting each unit area of the target, so the units are W/m^2 or J s^-1 m^-2. We can calculate it from the amount of energy striking the target of known area \(A\) per unit time:

\[I=\frac{P}{A}=\frac{E}{tA}\]

So given a certain amount of irradiance with a known photon wavelength, how much momentum does that light carry? First, figure out how many photons per time per area \(I_{pho}\) that irradiance represents:

\[\begin{eqnarray}I_{pho}&=&\frac{I}{E_{pho}}\\
&=& \frac{I\lambda}{hc}\end{eqnarray}\]

The dimension of photon irradiance is photons per time unit per area unit (photon s^-1 m^-2 in SI units).

Now a bit about momentum. Again, everyone knows Newton's second law \(\vec{F}=m\vec{a}\), but again this is a special case. Force is defined as the change in momentum per unit time:

\[\vec{F}=\frac{d\vec{p}}{dt}\]

For massive objects at sub-relativistic speeds, momentum is defined as the mass of the object times the velocity of the object. For relativistic conditions, we use the energy relation above, where energy still has the customary units J=kg m^2 s^-2. Working through the units of the energy equation in the special case of a photon, we find that momentum still has the same units as it does in sub-relativistic conditions.

So from this, a given amount of energy of photons carries a certain amount of momentum. A certain power of photons carries a flow of momentum per unit time, or in other words Force. A certain amount of momentum flow impinging on a certain area is the same as a force exerted on that area, or a Pressure \(\rho\). So, we can calculate the pressure exerted by a given irradiance of light from the intensity in photons/time/area, multiplied by the momentum of each photon, which gives momentum/time/area which equals force/area which equals pressure:

\[\begin{eqnarray}\rho&=&I_{pho}p_{pho}\\
&=&\left(\frac{I\lambda}{hc}\right)\left(\frac{h}{\lambda}\right)\\
&=&\frac{I}{c}
\end{eqnarray}\]

So the pressure exerted by the light is the irradiance of the light divided by the speed of light. That is amazingly simple, and notice that both Planck's constant and the wavelength cancel out. Now we can start plugging numbers.

First, let's do one of Clarke's space yachts. It weighs on the order of 1000kg and has on the order of 1km^2=1,000,000m^2 of sail, and it uses natural sunlight. How much force does it collect and what is its acceleration? Note that since the sails are reflective, the light is reversed in direction and has a momentum change of twice its original momentum, so we collect twice as much force as the momentum suggests.

\[\begin{eqnarray}I& & &\approx&1400\mbox{W/m}^2\\
c& & &=&299,792,458\mbox{m/s}\approx 3\times 10^8 \mbox{m/s}\\
\rho&=&2\frac{I}{c}&=&2\frac{1400}{3\times 10^8}=9.4\mu \mbox{N/m}^2\\
A& & &=&1,000,000 \mbox{m}^2\\
F&=&P_{pho}A&=&9.4\mbox{N}\\
m& & &=&1000 \mbox{kg}\\
a&=&\frac{F}{m}&=&\frac{9.4}{1000}=9.4\mbox{mm/s}^2\approx1.0\mbox{mg}
\end{eqnarray}\]

So the whole sail exerts a measly couple of newtons. Pulling on a ton of mass, we get almost a full milli-g of acceleration. Not a whole lot, but in order of magnitude of that described in the story.

Now we get into the big numbers. The game Adventure Capitalist teaches us to not be afraid of big numbers, so lets go get some. The Starshot project describes using gigawatts of power on spacecraft weighing grams. Among the enormous engineering challenges are:

• Focusing gigawatts of power onto square-meters sized targets over distances of billions of meters
• Having a reflective enough sail that the GW/m^2 incident power doesn't vaporize the sail
• Having a tough enough sail that the MW/m^2 absorbed power doesn't vaporize the sail
• Having a thin enough sail to stay within the mass budget
• Building a spacecraft with a useful payload, power source, and communications system that will work over interstellar distances and stay within the mass budget
• By the way, the mass budget is about 10g total.

Putting all of these aside, they discuss a spacecraft with a mass of about 10g, a sail area of about 16m^2, and multiple gigawatts of power focused on it. Let's go with 1GW/m^2, or 16GW total, to see what we get:

\[\begin{eqnarray}I& & &\approx&1\mbox{GW/m}^2\\
P_{pho}&=&2\frac{I}{c}&=&2\frac{1\times 10^9}{3\times 10^8}=6.66 \mbox{N/m}^2\\
A& & &=&16 \mbox{m}^2\\
F&=&P_{pho}A&=&16\times 6.66=106\mbox{N}\\
m& & &=&0.01 \mbox{kg}\\
a&=&\frac{F}{m}&=&\frac{106}{0.01}=10.6\mbox{km/s}^2\approx1080\mbox{g}
\end{eqnarray}\]

Well, that's some git-up-and-go, alright. How long does it take this to get to a target speed of a good chunk of the speed of light, say 60,000km/s (0.2\(c\))?

\[v=at\]

\[\begin{eqnarray}t&=&\frac{v}{a} \\
 &=&\frac{60,000,000}{10600}&=&5660 \mbox{s}
\end{eqnarray}\]

That's not too bad, a little over 1.5 hours, and real close to 1 LEO period, but not what the paper is discussing, which is on the order of 10 minutes. What acceleration is needed for that?

\[v=at\]

\[\begin{eqnarray}a&=&\frac{v}{t} \\
 &=&\frac{60,000,000}{600}&=&100 \mbox{km/s}^2\\
F&=&ma&=&0.01(100,000)=1000\mbox{N}\\
P&=&\frac{F}{A}&=&\frac{1000}{16}=62.5\mbox{N/m}^2\\
I&=&Pc&=62.5(300,000,000)=18.75\mbox{GW}
\end{eqnarray}\]

This is about 10000g. Now we are talking cannon-type acceleration. Since the acceleration is about 10 times greater, it will require 10 times the power, or a couple hundred gigawatts. This is a good fraction of the electricity usage of the United States, so it is a large but doable amount of power. We only need it for 10 minutes. The paper discusses putting the laser in space, which would require a gigawatt-scale power source in space. I suppose a couple of square kilometers of solar cells could do that.

If all of the engineering problems are solved, this could work. There isn't anything physically impossible about it. The engineering challenges are large, but the Starshot project claims that each can be solved incrementally. We don't have to shoot at stars first -- imagine getting back to Neptune in a couple of days, and then being able to orbit once you got there?

References

Lubin, P. A Roadmap To Interstellar Flight

Yukari 4 and nowcasting

I have been convinced to do Yukari 4, by of all people, my lovely wife Kellie. I have a theory about what is wrong, and how to fix it with one wire and a software change.

The Curiously Recurring Template Pattern

Go look up on Wikipedia what it is. I am going to talk about how I am having to use it.

I was doing fine with the Rocketometer analysis code in C++, using the NewMat library to handle matrices, with a Quaternion layer on top that I wrote myself. After five days of doing battle with various things, I finally got something that worked, but I was curious if this was the "best" way to do it. The C++ Standard Template Library didn't have anything directly related to matrices. The Boost library had a section called uBLAS, but the documentation for it kind of de-recommended itself. It suggested several alternatives, and the one that looked best is called Eigen.

Eigen is interesting in that it is entirely header files, containing almost all of its code in C++ templates. Templates are cool, mostly because when they are instantiated, the compiler gets to see the code in the context that it is used, and gets to inline and optimize it there. Specifically, Eigen contains a dynamic-sized matrix, but also is a template for fixed-sized vectors and matrices. I want to use these as much as possible because all vector sizes used in Rocketometer data analysis are known at compile-time, so the compiler can unroll loops and so on to best optimize the code.

However, templates do not mix with virtual methods, so I had to figure out how to make that work, since I used virtual methods to implement the physics model.  I had code that looks like this with NewMat:

class SimModel {
protected:
  /** Physics function. Calculates the derivative of the state with respect to time
  virtual ColumnVector fd_only(double t, const ColumnVector& x)=0;
  /** Measurement function. Calculates the measurement from the given state and time   */
  virtual ColumnVector g_only (double t, const ColumnVector& x, int k)=0;
  /** Physics function with process noise. Uses fd virtual function to calculate physics, then adds process noise.*/
  ColumnVector fd(double t, const ColumnVector& x,        const ColumnVector* v);
public:
  /** Measurement function with measurement noise. Uses g virtual function to calculate measurement, then adds measurement noise.   */
  ColumnVector g (double t, const ColumnVector& x, int k, const ColumnVector* w) {ColumnVector result=g_only (t,x,k);if(w)result+=*w;return result;};
}; 

But I wanted to adapt that to use Eigen, specifically with the fixed-length vectors, since the size of the state vector is determined by the problem and known at compile time. That means that ColumnVector has to go, to be replaced by Matrix<double,n,1> where n is a template parameter determining the size of the state vector. But what about the measurement? The purpose of the k parameter to g_only is to tell which of several kinds of measurements to use. For instance, in the Rocketometer problem, we have a measurement vector coming from the inertial and magnetic sensors, treated as a single 9-element vector. We also have measurements coming from radar or equivalent, treated as a 3-element vector. So, we need a template function g_only, which generates either a 9-element vector or a 3-element vector. You can't do that and have it be virtual, too. Basically, virtual functions are a runtime binding issue, while templates are a compile-time binding. So, I can't have a virtual g_only function, callable by the base class g function.

Enter the Curiously Repeating Template Pattern (CRTP). As it happens, this is something that I read about just a few days ago, just reading up on the C++ language in general. For us, the pattern goes something like this:

template<int n, class Derived> class SimModel {
public:
  template<int m> Matrix<double,m,1> g (double t, const Matrix<double,n,1>& x, int k, const Matrix<double,m,1>* w) {
    Matrix<double,m,1> result=static_cast<Derived*>(this)->template g_only<m>(t,x,k);
    if(w)result+=*w;
    return result;
  };
}; 

Note that g_only isn't even defined in this class template, only used. In fact, one of the weaknesses of CRTP is that it implies definitions without expressing them, so it is hard to document. Also note that extra template keyword there after the arrow operator. See here for details.

The derived class then looks like this:

template<int n> class RocketometerModel: public SimModel<n,class RocketometerModel<n>> {
  template<int m> Matrix<double,m,1> g_only(double t, const Matrix<double,n,1>& x, int k);
};

 So what happens is that the compiler

1. Parses the template for SimModel, but doesn't compile it, because it's a template, not actual code yet. Therefore it doesn't matter that g_only is undefined yet.
2. Parses the template for RocketometerModel, and again doesn't compile it.
3. Parses the main code, compiling as it goes along until it hits RocketometerModel. It instantiates and compiles RocketometerModel, in the process instantiating and compiling SimModel.
4. When SimModel is being instantiated and compiled, it has a call to RocketometerModel g_only, but this is ok, since that is available already, since step 2 has already happened.

Now the derived class might not be a template, it might be an actual class. In this case, the base class is instantiated and compiled with the derived class. In either case, everything is available before it is used, even though the code might look otherwise.

Now this part I will write in bold, so that Google can see it. The other curiously repeating template pattern is having to use the word .template when using (not defining) a template member function. This solves the error invalid operands of types ‘<unresolved overloaded function type>’ and ‘int’ to binary ‘operator<’ .

It appears that there is a weakness in the C++ operator precedence table, such that in certain cases it can't distinguish the opening angle bracket of a template parameter set from a normal compare-for-less-than. In order to disambiguate, you throw in the word template after the dot (it will also work after a pointer arrow -> if you are using one of those). I don't understand it completely myself, but Eigen uses this itself, which is how I found out about how it works in the first place.

I am gradually coming to the conclusion that Java did it right, with generics which are real compiled code. Java generics are enabled by the "everything is an object" model in which all objects descend from a common class. I am also beginning to think that Java did it right with operator overloading. Operator overloading, even when fully appropriate, like defining * to mean matrix or quaternion multiplication, is fun to use but a nightmare to implement. And, if it is implemented wrong, it might be left to the user to find out when he tries to do something the implementer did not foresee.

All in all, I give Eigen 4/5, especially recommended for new projects, but not for converting old projects. The biggest advantage is speed. What took IDL over an hour, took Java and C++ with NewMat about 4 minutes, but takes Eigen only 20 seconds. Also, templated matrix and vector sizes are nice, because they resolve matrix size mismatches at compile-time. Finally, zero-based component indexing is what I expect, and the reason I don't suggest converting old projects from NewMat. Also be aware that the Eigen quaternion library uses the convention \(\vec{v}_{to}=\mathbf{p}\vec{v}_{from}\mathbf{p}'\), which is fine and internally consistent, but not consistent with the math I had seen for quaternion derivatives. As a consequence, my code is liberally festooned with q.conjugate() and in some places q.conjugate().conjugate(). It's almost a case of two wrongs making a right, but not quite.

Detection of the Rotation of the Earth

Abstract: An MPU6050 6DoF MEMS sensor is used to measure the rotation of the Earth. The device is run pointed north for 1 minute, then south for 1 minute, taking 98samples/s during the runs. Actual rotation difference between north and south is 0.0063837°/s, taking into account the cosine(latitude) effect. Measurement is 0.0064822°/s±0.0015299°/s, representing a clear detection of the rotation.  The MPU6050 gyroscope is extremely accurate, probably sufficient for the Rocketometer mission.

This is also a review of the MPU6050 6DoF sensor. This part has a three-axis accelerometer with ranges ±2g, ±4g, ±8g, and ±16g, and a three-axis gyroscope with ranges ±250°/s, ±500°/s, ±1000°/s, and ±2000°/s. Since the intended mission is flying on a rocket, and certain rockets that may fly with this have been observed to accelerate at around 25g, it will need to be supplemented with a high-accelerometer, but the gyroscope will handle the 4.5Hz rotation expected.

The earth rotates 360° in 24h*60m*60s=86400s, or 1° in 240 seconds, or 0.00416666°/s. The gyroscope's most sensitive setting is ±250°/s and reads out at 16 bit resolution, resulting in 500°/s/65536DN=0.0076294°/s/DN, not quite enough to distinguish earth rotation from a standing stop, but enough to distinguish when one axis is pointing north, then south. Unfortunately, that presumes that the gyro is noise-free, which we will soon see is false.

The current experimental setup is on a breadboard connected to a version 1.1 Loginator by I2C at 400kHz, carefully aligned to true north by the following extremely accurate procedure: Since the walls of the secret underground laboratory are not aligned with true north, I pulled up the Google map of the lab and oriented the map such that the walls on the map were parallel to the real walls. The case of the phone was then aligned to true north. I put down a line of tape to mark this orientation. In this setup, the MPU6050 axes were aligned with +Z pointing down (thus we expect to see the 1g field read negative), the +Y axis pointing north first then south, and the +X axis pointing east first then west.

Of course, all measurements are contaminated with noise. This can often be beaten back by taking many measurements of the same thing. If you take into account a number of simplifying assumptions, the noise of the average of \(N\) measurements of the same quantity is \(\sigma/\sqrt{N}\). In short, if you take 100 measurements of the same thing, the average of them is expected to have 1/10 the noise of the original measurements. My previous efforts have taken data over very long stretches of time, hundreds of samples per second over hours. This time I decided to take data for 1 minute at a time. I sampled at 98samples/s (2samples/s were spent reading the pressure sensor, a topic for another day) for 1 minute with the Y axis pointing north, then 1 minute with the Y axis pointing south.

The estimated noise on each measurement, calculated with the standard deviation of all the measurements, was 11.02DN, or 0.084°/s, about 20 times that of the rotation of the earth, so it is obviously impossible to measure with one sample. But, I took 5880 samples, giving a predicted noise on the average of 0.14DN or 0.001°/s, plenty small enough to measure the rotation of the Earth. But did I? And if I did, why did I fail before?

Data:
Y north: -0.2217195°/s±0.0010972°/s (1σ)
Y south: -0.2282107°/s±0.0010972°/s (1σ)

Difference: 0.0064822°/s±0.0015299°/s (North is greater than South by this much)

Now, what is the expected value? First, is it positive or negative? Relative to inertial space, the device is rotating according to the right-hand rule around the Earth's axis. The device measures a right-handed rotation around an axis as positive, so the device should read more positive when pointing north than when pointing south, as it does.

Also, as mentioned above, the earth rotates at 0.00416°/s, but my Y axis is inclined 40° relative to the earth's rotation axis, so I should only expect to see \(\cos 40^\circ\) as much. Also, I would expect to see twice as much as that, because I am not comparing a standing stop to rotation, but rotation one way to rotation the other way.

Taking all this into account, the predicted measurement is....

0.0063837°/s.

My measurement clearly brackets this. In fact, the measurement is much better than I have any right to expect, being only 0.06σ above the expected value. I would have accepted any measurement with the proper sign and an error of less than 1σ.

Why did I fail before, with measurements taken over hours and hours? Gyro drift. Ideally, the zero point of the measurement would be zero DN, but we are happy if the zero point is just constant. But it's not. Temperature changes and other unmeasured effects cause the zero point to drift, and when measuring for hours and hours, this gyro drift swamps the signal I am trying to measure. To get a good rotation measurement, you want as many samples as possible, but taken over a relatively short time such that gyro drift is small.

So, back to the review. The MPU sensor is kind of noisy, with 11DN noise per sample, but can be read out sufficiently quickly and has sufficiently small gyro drift that it can measure the rotation of the Earth.

Gem of the Week - Kepler's and Newton's laws and universal gravitation.

The discovery by Kepler of his laws of planetary motion is one of the more amazing bits of observational science, made more amazing by the lack of tools which he had to work with. But, that's not our gem of the week. Instead, we will see how Newton deduced that there is such a concept as universal gravitation, and proved that it worked. Actually we won't see how he did it, but we will see how it can be done with modern techniques such as vectors.

Gem of the Week - Euler's Identity

I am going to present a new feature to all my 0 readers - the "Gem of the Week". This is a reprise of a sometime feature on my old private blog, "Chemical of the Day". I am expanding the topic somewhat from chemicals to anything I find interesting. None of these are necessarily news, but they might be.

This week, it's Euler's Identity.

Mathjax

Here is a new cool thing: MathJax

\[x=\frac{-b\pm\sqrt{b^2-4ac}}{2a} \M{A} \MM{P}{^-_i}\]

Mathjax is apparently a TeX implementation written entirely in Javascript. It looks like it scans the source code of your page, looks for delimited equations, then interprets the TeX within and renders it using math fonts.

Instructions for how to get it to work for Blogspot are here.

Now I get to go through all of my Kalman filter stuff and fix the math there into something actually readable.

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


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.

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]]

Intermediate Kalman Filtering theory, Part 2

Time for an example. We'll start really really simple, and just redo the linear velocity model.

First off, state vector. Basically the state vector is all the things that the system "remembers" about itself. Think about it as a simulation. Once we have a program, we run the simulation and freeze it at a certain point. What do we need to save so that when we restore, we have enough data to start the simulation back up? We'll use the cart on rails model as an example. We are working with a massive cart on smooth, level, perfectly frictionless rails. Therefore the cart is free to move in one dimension only. The very meaning of one dimension is that it takes only one number to describe the position, and we will call this number p. If we think of it as a function of time, we might say p(t). We need p in our state vector, but this does not yet completely describe the state. We also need to know the velocity. A cart at 4m down the track stopped is very different from a cart at 4m moving at 50m/s. As it turns out, this is all we need to know. This is all the cart "remembers" - everything else, such as our random gusts of wind, are instantaneous only. Any effect they have is only through changing the position and velocity. Therefore, <x>=<p,v>. Our state vector has two elements, p (in units of meters from the origin, positive on one side and negative on the other) and v (in units of meters per second, positive if moving one way and negative if the other).

Next, physics.We actually use Newton's first law here (normally problems start with the second law). The cart has no forces applied to it, so it continues in a straight line at constant speed. The derivative of the speed is zero (this is what constant speed means) and the derivative of the position is the speed, so we end up with this:

dv/dt=0
dp/dt=v

d<x>/dt=<x.v,0>

(An aside on vector notation. If the components of a vector <?> are named, I will specify the element named @ by ?.@ as if the vector were a structure in C. If the components are ordered, and they always are, I will call the first one ?[0], followed by ?[1] etc. Zero-based, again like C. So, for our state vector <x> above, we can talk about x.p, x.v, and note that x[0]=x.p . Vector components always have an order, but sometimes it is more clear to talk about the components by name if any, so I will freely use both notations. When I get to it, I will number matrix cells with two indexes, row first, then column, starting at 0. So, the upper-left cell of any matrix is ?[0,0].)

Believe it or not, we are done with the physics function.

F(<x>)=d<x>/dt=<x.v,0>.

Now for the observation function. The observation vector is a single number, the position along the track. So, we get

g(<x>)=<x.p>

Again, that's it.

Now for jacobians. We need a function that can calculate [Φ] given any <x>, and to do this we need derivatives. We are going to do the derivatives once on paper, code them up in a function, and then that function will provide the actual numerical derivative whenever we need it.

There are only four elements, so we can go nice and slow. In the upper left cell of the matrix, we take the derivative of the 0 component of the force function F[0](<x>)=x.v with respect to the zero component of the state vector x.p . Since this variable doesn't even occur in the formula, the derivative is zero and we have Φ[0,0]=0. Next, in the upper right [0,1] cell, we have component 0 of the force function F[0](<x>)=x.v still, but this time we are going with respect to the 1 component of the state vector x.v . The derivative of any variable with respect to itself is 1, so we get Φ[0,1]=1. Bottom left cell [1,0] uses F[1](<x>)=0 and x[0]=x.p so we have Φ[1,0]=0. To finish off, we have bottom right cell [1,1] which uses F[1](<x>)=0 and x[1]=x.v so Φ[1,1]=0. As it happens this time, all the cells are constant, so our physics matrix function can just return a constant in response to any state vector [Φ(<x>)]=[[0,1],[0,0]] for any <x>. As we shall see in our next example, things are not always so easy.

The jacobian for g() is even easier, but it is still a matrix. Any jacobian of any vector parameter function with a vector return value has as many columns as the input vector has elements, and as many rows as the output vector has elements. So, in this case, 1 row by 2 columns. For H[0,0], we use g[0](<x>)=x.p and x[0]=x.p, so H[0,0]=1. For H[0,1] we have g[0](<x>)=x.p and x[1]=x.v, so H[0,1]=0

To sum up, our problem is described as follows:

State vector <x>=<p,v>
Physics function F(<x>)=<x.v,0>
Observation function g(<x>)=x.p
Linearized physics matrix [Φ]=[[0,1],[0,0]]
Linearized observation matrix [H]=[[1,0]]

Also, a word about a priori initial guesses. You will note that we always need an initial guess of our state vector at step 0 <x^_0> as well as an initial guess of our uncertainty of our initial guess, [P_0]. For some problems, it doesn't matter so much what the initial guess is, since the filter converges rapidly. Sometimes, though, it doesn't, and when this happens, the filter gets farther and farther from the correct answer instead of closer, and we say that the filter diverges. If this happens in a real vehicle, the thing may diverge from its intended course and converge with a wall, causing the parts of the vehicle to diverge from each other. Let's agree right now not to diverge. Your filter can handle the truth, as much truth as you can give it. Give it as good of an initial guess as possible.

Now just because the notation is a little neater, we are going to use the Matlab language to write this example. You can use Gnu Octave to run this if you don't have and can't afford Matlab.

The program is structured as follows. The driver function starts with a piece to simulate the cart. It will create one data point every 100ms for 10s, for a total of 100 points. Next we will add some noise to create the measurements. Then, we create the a priori initial guess.

Finally, we run the filter loop. Each iteration of the filter loop will process one observation and create one state estimate. The time stamp of this estimate is exactly the time of the measurement. The numerical integrators will be in line, instead of in a separate function to start with. Just to see how it's done, we will do a numerical integration time step of 1ms, so we need 100 loops to get from one measurement to the next.
All the other equations will be pretty much as written.

After we have run the filter loop, we will compare the estimates with the true noise-free state we started with.


function nonlinear_velocity
  %Simulated actual trajectory. Stopped at the origin for first five seconds,
  %then constant acceleration such that position reaches 25m after 10
  %seconds.
 
  t=0:0.1:10; %Timestamp of each measurement
  size(t)
  pt=[zeros(1,50),(0:0.1:5).^2]; %True position
  size(pt)
 
  %We want random noise, but the *same* random noise each time we run
  randn('seed',3217);

  R=0.5^2; %Measurement noise covariance, 50cm on position
  pn=randn(size(t))*sqrt(R); %Position noise, standard deviation of 10cm.
  size(pn)
 
  Q=[0.01,0;0,0.01];  %Process noise covariance, 10cm/s on position and 10cm/s^2 on velocity
 
  z=pt+pn; %Observations
 
  xh_0=[0;0]; %Initial state vector guess
  xh=zeros(numel(xh_0),numel(t)); %History of all estimates (starts as initial estimate)
  xh(:,1)=xh_0;
 
  P_0=[1,0;0,1]; %Initial state vector uncertainty, 1m in position and 1m/s in velocity
  P=zeros(numel(xh_0),numel(xh_0),numel(t)); %History of all estimate covariances
  P(:,:,1)=P_0;  
 
  for i=1:100
    %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}>
   
    %One numerical integrator for both 1A and 1B, so that 1B can use the
    %intermediate results of 1A.
    dt=0.001;
    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]
    for j=1:100
      %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;
   
    %Record the current estimates
    xh(:,I)=xh_i;
    P(:,:,I)=P_i;
  end
 
  %Plot some interesting results
  figure(1)
  plot(t,pt,'r-',t,z,'k*',t,xh(1,:),'b-');
  figure(2)
  plot(t,pt-pt,'r-',t,pt-z,'k*',t,pt-xh(1,:),'b-');
end

%Physics function F(<x>). Takes a state vector, returns the derivative of
%the state vector. Primary physics model definition
function F=F(x)
  F=[x(2);0];
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(x)
  Phi=[0,1;0,0];
end

%Observation function g(<x>). Takes a state vector, returns an observation
%vector. Primary observation model definition
function g=g(x)
  g=x(1);
end

%Observation matrix [Phi(<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(x)
  H=[1,0];
end


And now for some results:

Hmm, not so good. The estimate starts lagging behind the truth, and never catches up. This is more because the model doesn't fit the actual cart (which in this case has a rocket on it which lights at t=5sec). If the model could not be improved, then we will have to sacrifice smoothness and increase the process noise. Try different (larger) values on the diagonal elements of [Q].

It doesn't really matter, this just supplies us with a good scaffold to hang a real nonlinear problem on.

Intermediate Kalman Filtering theory, Part 1

Real programmers are Not Afraid of Math
If you know calculus, great. If not, stop now, and run, don't walk, to your library or bookstore and beg, borrow, or buy a good calculus book. I recommend Calculus the Easy Way by Douglas Downing. Come back when you understand what a derivative is, and know how to calculate some easy ones. Also at least know what an integral is, even if you don't know how to calculate any, and read about the fundamental theorem, which basically states that a derivative can be reversed and that it is useful to do so. Knowing at least a smidgen of calculus will serve you long after your robot has decided to take a swim in the pond. It opens up a whole new world of problems and solutions, and that is useful for you to have a feel for whether a problem is solvable even in principle, even if you don't know how to solve it.

I'll wait.

OK, are we ready to proceed? Let's move on then.


Kalman filter processes like to use the following model:

<x_i>=[A]<x_{i-1}>+<w_{i-1}>
<z_i>=[H]<x_i>+<v_i>

Nonlinear Kalman Filter processes tend to use a different but related form:

<x_i>=f(<x_{i-1}>)+<w_{i-1}>
<z_i>=g(<x_i>)+<v_i></math>

where

• f(...) is the state transition function, which serves the same purpose as [A]. Given a previous state vector, the state transition function is used to transform it to the current state vector. So, this function takes a vector and returns a vector. This is more general than a pure matrix multiplication, and allows any kind of relation between this state and the next.
• g(...) is the observation function, which serves the same purpose as [H]. Given a state, the observation function is used to transform it to the observations. Likewise, a vector function with vector parameter.

HOWEVER

Most physical laws are in the form of vector differential equations, so we have:

d<x>/dt=F(<x>,t)

where

• F(...) is the physical law function, which is used to calculate the derivative of the state. This is then integrated to get a new function which describes the state in a continuous manner from the initial state and the laws. This function of state versus time is called the trajectory, since one of the basic uses of this is describing the actual trajectory of something, like a planet, moon, or robot. Remember not to confuse f() with F(). We almost never use or even have access to f().

Measurements are still taken at discrete instants. What we want is a way to shoehorn this continuous problem into a form suitable for the Kalman filter.
With sufficiently simple physics functions, you can crank out the integration as it shows in your textbooks and get the trajectory function directly. This is what Newton invented calculus for in the first place. However, for down-to-earth things like robots, the physics are usually too complicated to integrate directly. One of the things our filter will do for us is integrate numerically, so that we end up with (in principle) a list of times and the state vector for each time. This will be our trajectory function.


Linearization
If you just want the recipe, skip a couple of sections.


Nonlinear problems are hard. In fact, they can't be done. So instead, we do a modified problem and hope that it is close enough. This is the part where everyone goes off on Taylor series and such. I will just talk somewhat informally.
First, most physical processes are smooth. No discontinuities and no corners. Just nice sweeping curves. If you look at any of these curves closely enough, the relative radius of curvature gets bigger and bigger, and any small section eventually appears linear if you zoom in on it enough.

In order to take advantage of this, you have to look at the right part of a curve. We need a reasonably accurate initial condition and a correct dynamics and kinematics model. Now we can use the derivative of the curve to find the general direction of motion. We use the initial condition estimate and model to create a reference trajectory. We end up not estimating the trajectory directly, but the divergence from the reference.

So, we need a replacement for [A] and a way to update <x^_i> and <P_i>. The state vector is the easiest, so we'll talk about that. We know the physics model (If we don't, go find it!) so we can use it directly on our old estimate with numerical integration. I'll talk in detail about numerical integration in another post, but if you are a programmer, there is a good chance you have already written one. The simplest one is this simple:

• Find the rate of change of all the elements of the state vector at this point. That is, run F(<x^_{i-1}>), run the physics function on the old estimate.
• Find the time step size. This is t_i-t_{i-1}.
• Multiply the rate of change vector by the time step size. This is how much the vector has changed over the step.
• Add the change to the old estimate to get the new estimate.

All numerical integrators are just improvements and refinements on this. If the time step is short enough, you can just do this. One simple refinement is to break the time step into smaller sub-steps and do the integration repeatedly.

Replacing [A] is a bit harder. Skipping all the justifications and derivations, you need a physics matrix [Φ]. Finding it will require cranking out a bunch of derivatives by hand or computer, or writing a numerical differentiator. Then with [Φ] in hand, you can numerically integrate to get [A]. This is the "linearization" process talked about in textbooks.

[Φ] is a Jacobian matrix, a kind of vector derivative. Instead of taking the derivative with respect to time as you may be accustomed to, you take the derivative of each component of the physics function with respect to each of the other components. You end up with a matrix, where for each cell, you have the derivative of the component that is this cell's row number, with respect to the component that is this cell's column number. If you want to, you can think of it as fitting tangent hyperplane to the physics function, in the same way that an ordinary derivative fits a tangent line to a 1D function. This hyperplane is flat, so this is why we call it "linearized". We then project this hyperplane out as far as we need it, and use it to transform an identity matrix from one place on the trajectory to [A] on another. The math is pretty straight forward, it's just hard to imagine, especially with more than about 1 element in your state vector.

Once you have [A] in hand, you can easily find [P_i-] using the same formula as before.

We end up playing a similar Jacobian trick with the nonlinear observation. The Jacobian matrix of the observation function g(<x>) is dg/d<x>=[H]. No confusion with linear [H] because we use it the same way ever after.

Extended Kalman Filter algorithm

To remember, the five equations of the linear Kalman filter are

1. <x^_i->=[A]<x^_{i-1}> 
2. [P_i-]=[A][P_{i-1}][A']+[Q]
3. [K_i]=[P_i-][H']([H][P_i-][H']+[R])^-1 
4. <x_i>=<x^_i->+[K_i](<z_i>-[H]<x^_i->)
5. [P_i]=([1]-[K_i][H])[P_i-]

Equations 1 and 2 are called the time update while equations 3 through 5 are called the measurement update.

Plugging in what we have above into this form, we get the following. Don't panic too much, it will all make sense with a concrete example.

Time update

1a. <x^_i->=∫ F(<x(t)>,t) dt from t=t_{i-1} to t_i with initial condition <x(t_{i-1})>=<x_{i-1}>
1b. [A(t_i,t_{i-1})]=∫ [Φ(<x(t)>,t)][A(t,t_{i-1})] dt from t=t_{i-1} to t_i with initial condition [A(t_{i-1},t_{i-1})]=[1]

Basically these integrations cover the time from one step to the next, generate our [A] for us, and turn this from a continuous time problem back into a step problem like we had before. We do these integrations numerically and in lockstep, so that we can use the intermediate result <x(t)> from 1a where we need it in 1b.

1c. <X^_i->=<0>
2. [P_i-]=[A(t_i,t_{i-1})][P_{i-1}][A(t_i,t_{i-1})']+[Q]

Equation 1c is where things start going different. We are estimating the deviation from the reference trajectory, but our update follows the reference trajectory perfectly, so our initial estimate of the deviation is zero. Equation 2 is exactly like the old form.

Measurement update
 

3a. [H_i]=dg(<x^_i->,t_i)/d<x^_i-> Linearized observation matrix
3b. [K_i]=[P_i-][H_i']([H_i][P_i-][H_i']+[R])^-1 Kalman gain
4a. <Z_i>=<z_i>-G(<x^_i->,t_i) Measurement deviation
4b. <X^_i>=<X^_i->+[K_i](<Z_i>-[H_i]<X^_i-> Measurement update of state deviation
4c. <x^_i>=<x^_i->+<X^_i> Update of reference state vector
5. [P_i]=([1]-[K_i][H_i])[P_i-] Measurement update of state covariance

My teacher calls this is the extended Kalman filter, in distinction from the linearized Kalman filter, because the reference trajectory is updated every time around. To do the linearized filter, don't reset the state deviation to zero each time (just at the start) and don't add the state deviation to the reference state at the end. If the state deviation gets too big, you will need to do a rectification every once in a while, but not at every measurement, where you do these things. The extended filter basically does rectification every time.

Beginners Kalman Filtering Theory, Part 5

Pay attention here, because we are finally going to develop something practical - The Kalman Velocity Model

This is about the only problem I have found which both has a practical use, and actually fits the linear model.

Let's imagine a rail car on a level frictionless rail. The car is unpowered and uncontrollable, but is subject to unpredictable accelerations (say from gusts of wind). We want to know the position and velocity of the car, but our only measuring device is a tape measure laid out along the track. Once again we can measure the position of the car to arbitrary position, but only finite accuracy.
First, let's look at the process model again:

<x_i>=[A]<x_{i-1}>+<w_i>
<z_i>=[H]<x_i>+<v_i>

Now that we are considering motion, we need to consider time. The spacing between two measurements i-1 and i we will call Δt (Delta-t). For now we will consider Δt to be constant, meaning that we have equally-spaced measurements.

Now I am beginning to curse Blogspot and it's lack of math typography. To show a matrix, I will do the following. If the matrix is

[a b]
[c d]

I will write it as [[a,b],[c,d]] or in other words, rows are the inside brackets and the whole matrix is a bunch of stacked rows. If I have a vector, I will say

[a]
[b]
[c]

is the same as <a,b,c>, which is the same as [[a],[b],[c]]. All vectors are column vectors unless otherwise stated. Even a vector written <a,b,c> is a column vector.

The state of the vehicle is completely determined by its current position p, and velocity v, so its state vector is:

x=<p,v>

The state transition matrix is two-by-two, since the state is two-element. In simple linear equation language, we have this:

p_{i}= p_{i-1} +Δt v_{i-1}
v_{i}=                   v_{i-1}

Converting this to a matrix equation gets us this:

<p,v>=[[1,Δt],[0,1]]<p_{i-1},v_{i-1}>
<x_i>=[A]<x_{i-1}>

so we have a state transition matrix:

[A]=[[1,Δt],[0,1]]

For the observation matrix, we are taking the two-component state and converting it to one measurement, so the matrix is one-by-two.

<z_i>=(1)p_i+(0)v_i
<z_i>=[[1,0]]<p_i,v_i>

so the observation matrix is:

[H]=[[1,0]]

Process Noise Model
We represent the unpredictable accleration component with the process noise covariance matrix. The process noise is a random vector variable with two elements, so its covariance is a two-by-two matrix. I will borrow the process noise model from Wikipedia. In between any two measurements, the unknown process noise acceleration is presumed to be constant a_i. By integrating this over the time step, we see that the velocity is changed by a_iΔt and by integrating twice we see that the position is changed by 0.5a_iΔt².

So in mathematical terms, we get

<x_i>=[A]<x_{i-1}>+<w_i></math>
<w_i>=<0.5a_iΔt²,a_iΔt>
<w_i>=<0.5Δt²,Δt>a_i
<g>=<0.5Δt²,Δt>
<x_i>=[A]<x_{i-1}>+<g>a_i

This comes straight from Wikipedia underived. The process noise covariance matrix is

[Q]=<g><g'>σ_a²=[[0.25Δt⁴, 0.5Δt³],[0.5Δt³,Δt²]]σ_a²

Here I use <?'> for the row vector form of column vector <?>. I don't know the exact derivation, but basically it is saying that since the standard deviation is known, and the variance is required, you just square the standard deviation. Since you can't directly square a vector, the idiom <g>²=<g><g'> is used.

As we look through the filter equations, it turns out that [Q] is always added to the projected value of [P], so it doesn't matter that [Q] is singular by itself. The parameter σ_a has units of acceleration, and represents the expected magnitude of the random velocity change per unit time, while a_i represents the unknown actual acceleration.

In this case the process noise is a real part of the model, and not an ugly hack or tuning parameter. We defined the model with this random acceleration, and this process noise matrix describes it in math. For real systems such as EVE, the process noise is still an ugly hack.

This form of the process noise is preferred, because the process noise parameter then is decoupled from the timestep size.

The measurement is a scalar, so its covariance is scalar also.

R=σ_z²

We can then apply these matrices directly to the Kalman filter form.

Sensor fusion and sensor anti-fusion
In this  simple model, we have only one kind of sensor, for position, but we are able to recover both position and velocity. This is possible because our physics model [A], simple as it is, is complete and captures the whole state vector. This concept is important. You don't necessarily need a sensor for every component of your state vector. This is sensor anti-fusion, I guess. It is a complementary concept to sensor fusion.

With sensor fusion, sensors of two different types are used to measure the same parts of the state vector. We'll get into exactly how to do this in a later post, but let's just talk about goals for a moment. Suppose your robot has a 6DOF IMU and a GPS. You use the IMU for accuracy over short distances and times, and the GPS to keep things aligned with the real world, and keep the IMU from wandering off too far.

Now, you are probably not going to measure the GPS and IMU at the same time. So, for some steps, perhaps most steps, the IMU measurement is available and the GPS is not. Other steps, the GPS is available and the IMU is not. Perhaps there are rare times when both measurements are taken simultaneously.

What you do in this case is use a different observation model, and therefore a different [H] depending on what combinations of sensors are available at a particular instant. When you use the IMU, you have an [H] (or actually the nonlinear equivalent of [H]) which only uses the IMU. You also have the measurement uncertainty of the IMU in its own [R]. When you use the GPS, you use a different [H]. And, when both are available simultaneously, you could use an [H] that incorporates both. We'll see why we don't do that in a later post.

With the right [H] for each measurement and the right [A] (or its nonlinear equivalent as we shall see) for the physics, combined with the correct measurement noise for each measurement, the filter does all the bookkeeping and weighting automatically to incorporate multiple different kinds of sensors into the same estimate. It automatically weights each measurement to use it as best as possible. If you don't lie with [R], it is able to weigh things appropriately without any further intervention. This is what is meant by sensor fusion.

EVE example
Now this process noise we have applied is just a model, just a fudge factor to keep the filter from getting too satisfied with itself. The filter thinks the process has noise, but in reality that noise is the process not following the model. I am not prepared to tell the sun what to do.

Here's an example using the velocity process above, applied to the EVE instrument. The filter uses this process noise model, but the real data does not. During a measurement sequence of 1000 measurements, the Sun generates a rock solid constant 20 units of flux for the first half. At the exact halfway point, we see a flare begin. This flare has a maximum amplitude of 20 units, and then decays away again.

The EVE instrument is modeled as having a Gaussian uncorrelated measurement noise with a standard deviation of one unit, with the following exception: On a randomly selected 1% of the measurements, the standard deviation is five units. This represents an actual phenomenon seen on EVE.

The heavily marked points are the 1% "popcorn" (There might not be exactly 10, since they are thrown in at random). The red line represents reality again, and the blue line represents the filter output. We notice immediately that the filter has less noise than the original data, so the filter is working. We also notice that while the filter output follows the popcorn, it is not nearly as bad as the original data itself.

Here is a plot of the estimate difference from the real measurement. The heavy line is the filter residual, and the light lines are the ±3-sigma uncertainty lines.

The problem here is that the filter takes a while to believe that the process really changed, so it lags a bit behind the actual process. This is that large downward spike we see at the half way point. Other than that, the filter has a standard deviation of about a third that of the original data, and the 3-sigma lines include the actual value almost all the time, as it should.

The process includes a notion of velocity, the rate of change of the flux, but this is not a measurement that EVE actually makes. Even so, the filter provides a velocity estimate automatically. We could estimate it, but no one has asked for it. Perhaps if the scientists know that it is available, they will want it.

This is very educational as to what the problem is. The real flux (literally) explodes, with a step jump in velocity. The filter takes a while to follow, and by the time it has followed, the flux velocity is dropping rapidly, and once again the filter has to chase it.

Testing your filter
Unless you are a much better programmer than me, and I don't think you are, when you originally code this up, it is going to have bugs. Now the normal way to debug is to trace the program flow, and at every line, or at least every critical line, predict what the program is going to do then test it by letting it execute. However, if you don't understand the algorithm, how can you do this prediction?

This is why we are going step by step, with gradually more elaborate filters, each built off the last.I gave IDL code for a scalar Kalman filter in a previous post. This simplest filter was devolved to the point that all the magic in equations 3 and 5 has been distilled out, leaving only an obvious averager. Then, as you add refinements one at a time, test frequently. When it breaks (and it will) you know what part is broken: the last one you added, or at least an interaction between that part and the rest of the code.

All of these charts were generated by that script, gradually evolved to do the more and more elaborate filters. I could (well actually I can't, the code has evolved away from those points) give you code for each filter, but I won't, because that's not a favor to you. You need to work with the code to understand it, and the best way is to evolve the code a step at a time as I have. You are guided by the examples, but you put in the effort yourself to follow them. It's all very Socratic.

Even if you don't have IDL, you can program the simpler models even in a spreadsheet, and the more complicated one in your favorite data-handling language, or even general purpose language like C, Java, or Perl. If your language has no native charting abilities, have your filter write its output into a CSV, then suck it into a spreadsheet. There's no excuse, as C, Java, Perl, and OpenOffice are all free for the taking and available on almost all platforms (even your robot, if you write in C).

As you progress, you will come to the point where you will write a general Kalman filter function, which can handle any process model thrown at it. At this point, you will then be debugging your models. Before you try to debug on your robot, try using [A] and some initial conditions to simulate a robot trajectory and using [H] to generate measurements at frequent intervals. This way, the normally unknowable True State is in fact known, and you can compare your estimate with the Truth, in a way which is almost impossible otherwise.

The usefulness of this model
By itself, we can use this model to estimate the rate of change of any variable that we can measure. If you have a thermometer, you can use the filter to not only reduce the measurement noise, but also create out of thin air a measurement of the rate of change. Likewise the voltage and charge state of your battery, the altitude and climb/sink rate measured by an ultrasonic pinger used as an altimeter, or any number of things.

Further, when I describe the nonlinear filter and IMU model, you will see this model embedded in that one. The filter will integrate the acceleration to get velocity and position, but the acceleration it integrates will not be the direct measurement of the sensor, but an estimate based on the linear velocity model.

Code
Since this is a useful result in itself, code for it is posted here. Partly to check your work, but mostly so I have a home for it.


function [xh,P]=kalman_vel(xh0,P0,t,z,sigma_a,R)
  xh=zeros(numel(xh0),numel(t));
  xh(:,1)=xh0;
  P=zeros(numel(xh0),numel(xh0),numel(t));
  P(:,:,1)=P0;
  H=[1,0];
 
  for I=2:numel(t)
    i=I-1;
    xh_im1=xh(:,I-1);
    P_im1=P(:,:,I-1);
    dt=t(I)-t(I-1);
    A=[1,dt;0,1];
    Q=[dt.^4/4,dt.^3/2;dt.^3/2,dt.^2]*sigma_a;
   
    xh_im=A*xh_im1;
    P_im=A*P_im1*A'+Q;
    K=P_im*H'*inv(H*P_im*H'+R);
    xh_i=xh_im+K*(z(I)-H*xh_im);
    P_i=(eye(size(P0))-K*H)*P_im;
   
    xh(:,I)=xh_i;
    P(:,:,I)=P_i;
  end
end
   


Input parameters:

• xh0 - Initial guess of state vector
• P0 - Initial guess of state vector estimate covariance
• t - list of times - 1D array
• z - list of measurements of position - 1D array same size as t
• sigma_a - 1-sigma estimated process noise magnitude - scalar
• R - 1-sigma measurement noise - scalar

Output parameters:

• xh - List of state vector estimates. First row is "position" and second row is rate of change.
• P - List of covariance matrices.

Beginners Kalman Filtering Theory, Part 4

Process Noise

If we know that the constant really is a constant, then what we did before is just fine, but we might as well just do an average. Suppose however, that our constant isn't really constant. Suppose it has a step in it. If you think about the filter as an averager, an averager never forgets. The filter result will end up the weighted average between the measurements before the step and the measurements after.

So, what we can do is give the filter a little uncertainty, a little nagging doubt, about its previous results. We tell it that its estimate of the estimate error covariance is a little bit wrong. We create a little bit of doubt that the system really is a rock solid constant. We add process noise.

Reiterating the Kalman filter again in all its glory, let's reduce it again, but this time include the process noise term.

1. <x^_i->=[A]<x_{i-1}>
2. [P_i-]=[A][P_{i-1}][A']+[Q]
3. [K_i]=[P_i-][H']([H][P_i-][H']+[R])^-1
4. <x_i^>=<x_i^->+[K_i](<z_i>-[H]<x_i^->)
5. [P_i]=([1]-[K_i][H])[P_i-]

Once again, a cancelfest:

1. x^_i-=x_{i-1}
2. P_i-=P_{i-1}+Q
3. K_i=P_i-/(P_i-+R)
4. x^_i=x^_i-+K_i(z_i-x^_i-)
5. P_i=(1-K_i)P_i^-

Now the estimate covariance never collapses to zero. In fact, it never decreases below the process covariance Q, which is what we would expect. If the process really is uncertain, our estimate can never be less than that uncertainty.

Lets sic this on the true constant first, to see what we give up:

The final estimate is 0.5055V±0.0520V. Naturally our uncertainty is a lot worse, since the filter thinks that the process really is uncertain about what its state is. It weighs the previous estimate quite a bit less than it did when the process was rock solid. As it turns out, once the filter converges, it weighs the previous estimate about three times heavier than the current measurement, so in a way its memory only extends for three measurements. The filter in this case is a moving average of the recent past, where older measurements' weight decays exponentially.

Let's see how it does now against a step in the constant:

Since it only remembers back about three measurements, it only takes about three measurements to start tracking the new value.

Process noise represents our uncertainty about what the process really is doing, uncertainty about our model. But, how can you assign a standard deviation to "maybe the constant has a step in it"? It certainly doesn't have a normal distribution. The right answer is to fix the process model, but maybe we can't. In this case, we use the process noise level as just a tuning parameter. We can tune it to get the right balance between noise filtering and responsiveness to change. In the above examples, the process noise is rather large, only 1/sqrt(10) of the measurement noise (Q=0.001, R=0.01). Let's see what happens as we turn the process noise down. All of these have the same measurement covariance, only the process covariance changes.

Q=1e-4, memory about 10 steps

Q=1e-5, memory about 30 steps

Q=1e-6, memory about 100 steps

So we see that as we turn the process noise down, the estimate in the flat part becomes smoother, in exchange for the response to the new data becoming longer.

This kind of tuning is a judgement call. It's actually an ugly hack, and should be avoided if possible, but it may not be possible.