This page provides some more detail on the pressure only version of the Kalman filter.
While I made a few other changes to the code to remove the engineering unit conversions, the major change was in the Kalman filter code.
Here is the source code
Note that the data storage is defined elsewhere. It isn't much. 4 bytes each to store the altitude, velocity, and acceleration state variables plus another 4 bytes as a temporary location to store stuff. A single byte variable is also needed by the the right shift routine.
The first routine performs the initialization of the filter. This is really very simple. Since the rocket isn't moving when the altimeter is powered up (If it is moving, you have serious problems.) velocity and acceleration are set to zero. Then the current altitude is determined through a call to an external routine and this is stored in the altitude state variable. That is it.
The initialization code is followed by a couple of utility routines. The first is a multiply/accumulate routine. Since I needed to multiply by a Kalman gain and then add the result 6 times in the original code, I wrote this to save space. It isn't called as much now but it still saves space. So it stays.
Then there is a routine that performs a right shift.
Then there is the routine to perform the Kalman filter operation. This does all of the computations required for a single step. It is currently written assuming that it will be run 128 times per second. If you want to use a different sample rate, considerable changes are required.
The first step in the Kalman filter is the state propagation. This predicts what the current state is using the state at the previous sample. This requires that the velocity be multiplied by the time between samples. This is the same as dividing by 128. To speed things up a bit, I shift right by one byte while copying the velocity to a temporary location before shifting it back left 1 bit. This is a lot faster than simply shifting right by 7 bits. This value is then added to the altitude.
Next up is to compute 1/2*a*t^2. This requires a right shift of 15 bits so I once again use a byte shift while copying and then correct it with a 1 bit left shift. This also gets added to the altitude.
Similar computations result in adding a*t to the velocity and the state propagation is complete.
Next is the state correction. This is where the filter compares the measured altitude to the predicted. This difference is then used after multiplication by the Kalman gains, to correct the state variables.
The first step is a call to the routine that gets the altitude. When using the full strength version of this code that uses both acceleration and pressure altitude, this routine also converts the altitude into meters. This is because the units have to be consistent. Now that I don't have an acceleration reading to work into the mix, I just leave this as ADC counts. This has a couple of consequences.
First of all is that the units are a bit odd. Altitude in counts, velocity in counts/second, and acceleration in counts/second/second. Odd but since the apogee criteria is a sign change in the velocity rather than a particular value, it works just fine. Another consequence is that the signs change. While altitude goes up the pressure goes down. The result is that now velocity will first go negative and then cross over to positive at apogee. The launch and apogee detect tests need to change to reflect this sign change. Another way to fix this would be to simply subtract the ADC reading from its full scale value.
One very subtle consequence of this change is that now the dynamic model doesn't quite match reality. Because the pressure/altitude relationship is very non-linear, we now have a disconnect between the pressure measurement and the model. Because the rate of change of the pressure is so low around the time of apogee, I do not expect this to have a significant impact on apogee detection performance.
The routine to get the pressure reading should put the ADC result in the two most significant bytes of T0-T3 and clear the lower two bytes.
The difference between the estimated and measured altitude is then computed. This value is used to correct the estimates after multiplication by the Kalman gains. This is where the big changes are in this code.
I decided to try eliminating the 32X16 multiply and using bit shifts instead. Since the Kalman gains are not powers of 2, this results in errors in the Kalman gains. Tests using recorded data show that the filter is robust enough to tolerate these gain errors. Which is kind of surprising.
I just load up the required shift count and call the multiply/accumulate code. Because the result of the multiplication is left in the temp location afterwards, I don't need to shift it as much for the next multiplication. This way I can use gains of 1/32, 1/32, and 1/64 by shifting five bits, zero bits, and then one bit.
That completes the state correction and the Kalman filter update.
I left the call to get the acceleration measurement at the end so that I could still record the acceleration data in the serial EEPROM. It is not required by the Kalman code.
The complete update (excluding the calls to get the measurements) takes only 200 instructions to execute. With the 4MHz clock this is 200 microseconds. Very fast. The code size and data storage requirements are also quite reasonable.