Hybrids and altimeters

[jderimig]

I'd love to see a snippet of code (preferably in C...) for that. All of the code I've seen so far seems a lot more complex than it should given that there is only one sensor (baro) and an error-free model. Dave's core code is in PIC assembly, I don't do PIC (at least, yet...).

Here's one simplistic implementation

void KalmanUpdateState(int32_t aglAltMeas, int16_t accelMeas)
{
	// calculate apriori state estimate from last updated state
	//
	// calculate next estimate
	int32_t alt, vel ;
	int32_t accel ;
	int32_t res_alt ;
	int32_t res_accel ;

	// save last updated estimate in dummy variables
	alt = strState.strData.i32KalAlt ;
	vel = strState.strData.i32KalVel ;
	accel = strState.strData.i32KalAccel ;
	
	// calculate apriori estimate of next state
	strState.strData.i32KalAlt = KGA1*alt + KV1*vel + KDV1*accel ;
	strState.strData.i32KalAlt /= KGA2 ;
	strState.strData.i32KalVel = KGA3*vel + accel ;
	strState.strData.i32KalVel /= KGA4 ;

	// calculate measurement residuals
	res_alt = (int32_t)aglAltMeas - strState.strData.i32KalAlt ;
	res_accel = (accelMeas*KK)/K2 - strState.strData.i32KalAccel ;

	// update prediction by multiply measurement residuals by Kalman gains
	strState.strData.i32KalAlt += (KGAIN00*res_alt + KGAIN01*res_accel)/TEN3 ;
	strState.strData.i32KalVel += (KGAIN10*res_alt + KGAIN11*res_accel)/TEN3 ;
	strState.strData.i32KalAccel += (KGAIN20*res_alt + KGAIN21*res_accel)/TEN3 ;

}

 

[cerving]

Thanks, this looks like what I've been looking for.

Here's one simplistic implementation

void KalmanUpdateState(int32_t aglAltMeas, int16_t accelMeas)
{
	// calculate apriori state estimate from last updated state
	//
	// calculate next estimate
	int32_t alt, vel ;
	int32_t accel ;
	int32_t res_alt ;
	int32_t res_accel ;

	// save last updated estimate in dummy variables
	alt = strState.strData.i32KalAlt ;
	vel = strState.strData.i32KalVel ;
	accel = strState.strData.i32KalAccel ;
	
	// calculate apriori estimate of next state
	strState.strData.i32KalAlt = KGA1*alt + KV1*vel + KDV1*accel ;
	strState.strData.i32KalAlt /= KGA2 ;
	strState.strData.i32KalVel = KGA3*vel + accel ;
	strState.strData.i32KalVel /= KGA4 ;

	// calculate measurement residuals
	res_alt = (int32_t)aglAltMeas - strState.strData.i32KalAlt ;
	res_accel = (accelMeas*KK)/K2 - strState.strData.i32KalAccel ;

	// update prediction by multiply measurement residuals by Kalman gains
	strState.strData.i32KalAlt += (KGAIN00*res_alt + KGAIN01*res_accel)/TEN3 ;
	strState.strData.i32KalVel += (KGAIN10*res_alt + KGAIN11*res_accel)/TEN3 ;
	strState.strData.i32KalAccel += (KGAIN20*res_alt + KGAIN21*res_accel)/TEN3 ;

}

 

[jderimig]

Thanks, this looks like what I've been looking for.

Forgot to mention, this is code where there is a altitude (baro) and acceleration measurement.

 

[cerving]

I noticed that, I think it could be adapted easily enough for baro-only. I also noticed that you have constant values for the gains, how did you arrive at them?

Forgot to mention, this is code where there is a altitude (baro) and acceleration measurement.

 

[jderimig]

I noticed that, I think it could be adapted easily enough for baro-only. I also noticed that you have constant values for the gains, how did you arrive at them?

Not a simple answer. Textbook methods, empirical data, Minitab and Matlab.

 

[cerving]

Dave's code does include a separate routine for computing the Kalman covariance matrix, and conveniently it's in C. That might be the missing piece...

 

[UhClem]

Dave's code does include a separate routine for computing the Kalman covariance matrix, and conveniently it's in C. That might be the missing piece...

These days I use Scilab.

A=[ 1 dt dt*dt/2;
    0 1  dt;
    0 0   1];

H1=[ 1 0 0;          // measurement matrix
     0 0 1];
Q=[0 0 0;           // Process noise
   0 0 0;
   0 0 pnoise];

R=[12.5*12.5   0;  // Measurement noise
    0        8*8]; // measured
I = eye(3,3);
P = I;             // Initialize  covariance matrix (a WAG)

// The gains should converge within 1,000 iterations or so.
for i = 1:1000
  P = A*P*A' + Q;          // Covariance update
  K1 = P*H1'*inv(H1*P*H1' + R); // Kalman gain
  P = (I - K1*H1)*P;         // Covariance correction
end
disp(P);
disp(K1);

 

[keithp]

I'd love to see a snippet of code (preferably in C...) for that

Here's a link to the MicroPeak code. All of our firmware is GPLv2 licensed, so you can get the source, fix it, reflash your boards or even build your own hardware using our code (as long as you respect the terms of the license).

https://git.gag.com/?p=fw/altos;a=blob;f=src/micropeak/ao_microkalman.c;hb=HEAD

As you can see, the 'predict' phase uses three multiplies and three adds, and the 'correct' phase does one subtract and then another three adds and three multiplies. Because all of the error values are constant, we can pre-compute all of the coefficients at compile time, saving the expense of tracking the error matrices.