include p16f628.inc
include data.inc
include adc.inc
include eedata.inc
global KALMAN_STEP,kalman_init
extern get_altitude, get_acceleration, eeread
extern beep_alt
;; Copyright 2004 David W. Schultz
;;
;; Quick and dirty version of Kalman filter for PIC16 processors.
;;
;; This version uses only the pressure data and has been altered for speed
;; at the expense of accuracy. This was done by replacing the 32X16
;; multiplications with simple bit shifts. This causes errors in the Kalman
;; gains because they are not precide powers of two.
;; The Kalman filter seems ot be robust enough that it can tolerate this error.
;; Kalman gains are computed by a "C" application and are determined
;; by sample rate, measurement noise, and model noise. To compute
;; gains, select sample rate, measure sensor noise
;; (standard deviation), and select measurement/model noise ratio. I
;; recomend a ratio of 1000 to 10000. Input parameters into kgain31.exe and
;; note results. Gains shown here are based on 128 SPS, noise of 1 lsb
;; for pressure and a noise ratio of 1,000.
;;
;; If you need to use a differnt sample rate, noise ratio, or have more noise
;; in your data, you will need to recompute the gains and alter the code to match.
;; Don't forget that the initial part of the state update assumes 128 SPS. This
;; will have to be recoded (changing the bit shifts) for different sample rates.
;;
;; A note on number representation.
;;
;; Altitude, velocity, and acceleration are stored in 16.16 signed fixed
;; point format. The two most significant bytes represent the integer
;; portion of the number and the two LSB's the fractional portion. It's
;; just like base ten if you are missing 8 fingers. :-)
;;
;; All of the data storage allocation is handled in a separate file and an
;; include file takes care of the references here.
;;
;; Data storage required:
;;
;; ALTB0, ALTB1, ALTB2, ALTB3 -- 4 bytes for altitude
;; VELB0, VELB1, VELB2, VELB3 -- 4 bytes for velocity
;; ACCELB0, ACCELB1, ACCELB2, ACCELB3 -- 4 bytes for acceleration
;;
;; That covers the state variables. They must not be changed anywhere else
;; or very bad things will happen.
;;
;; T0, T1, T2, T3 -- Temporary storage
;; LOOPCOUNT - 1 byte used for a loop counter in the shift code
;; SIGN - 1 byte used in the shift loop
;;
;; The temporary storage can be used for other purposes either before or
;; after this routine is called. Do not allow any routine you call from
;; here to alter it. Except of course for the get altitude routine.
;;
code
;; Initialization of the Kalman filter is pretty simple. Just set
;; the velocity and acceleration to zero. Then get the current altitude
;; from the pressure sensor to set altitude.
;;
kalman_init:
clrf VELB0
clrf VELB1
clrf VELB2
clrf VELB3
clrf ACCELB0
clrf ACCELB1
clrf ACCELB2
clrf ACCELB3
;; Read current altitude
;; The ADC result should be in T0 (MSB) and T1 (LSB). T2 and T3 should be
;; zero.
call get_altitude ; result is in T0-3
movf T0,W ; copy to altitude state variable
movwf ALTB0
movf T1,W
movwf ALTB1
movf T2,W
movwf ALTB2
movf T3,W
movwf ALTB3
;; set timer tick to zero
;; This isn't used by the Kalman filter code. This was just a handy spot
;; to put this.
clrf TICKB0
clrf TICKB1
clrf TICKB2
return
;; Multiply/accumulate code. This routine is used by the state
;; correction code.
;;
;; For this"Kalman light" version, the multiply code is gone.
;; Instead there is a call to the right shift code
;; Before calling, T0-T3 should contain the inovation. W
;; the number of bits to shift T0-T3 by, and FSR should point to the
;; location to add the result to.
;;
;; NOTE: MSB of multi-precision numbers is in xxxB0 which has the
;; lowest address. FSR should point to the LSB (xxxB3) of the target.
;;
;;
KALMAC:
movwf LOOPCOUNT
call RSTEMP ; shift temp right W bits
;;
;; A secondary entry point that skips the right shift.
;;
KALMAC1:
;; crap! I hate the fact that this stupid chip doesn't have an add
;; with carry. 32 bit addition. The hard way.
movf T3,W ; add result to altitude
addwf INDF,F
decf FSR,F ; point at next byte
movf T2,W
btfsc STATUS,C ; handle the carry
incfsz T2,W
addwf INDF,F
decf FSR,F
movf T1,W
btfsc STATUS,C
incfsz T1,W
addwf INDF,F
decf FSR,F
movf T0,W
btfsc STATUS,C
incfsz T0,W
addwf INDF,F
return
;;
;;
;; Arithmetic right shift 32 bit T0-T3, LOOPCOUNT places
;;
RSTEMP: movf T0,W ; Copy MSB to use for sign extension
movwf SIGN
RSLOOP: rlf SIGN,W ; Pick up the sign bit in C
rrf T0,F
rrf T1,F
rrf T2,F
rrf T3,F
decfsz LOOPCOUNT,F
goto RSLOOP
return
;;
;; Begin state update
;;
;; Start by computing Xt+1 = X + V*T + A*T*T/2
;;
;; Since T is a fractional power of two, we do this by shifting bits
;;
;; First make a temporary copy of the last velocity estimate and
;; divide the copy by 128.
;;
KALMAN_STEP:
;; Make a copy, shift by one byte (8 bits) to divide by 256
movf VELB2,W
movwf T3
movf VELB1,W
movwf T2
movf VELB0,W
movwf T1
;; Perform sign extension on the msb...
movlw 0 ; sign extension
btfsc T1,7
movlw 0xff
movwf T0
;; Now shift left one bit to get to back to a total of dividing by 128
rlf VELB3,W ; pick up the msb of VELB3 for the left shift
rlf T3,F
rlf T2,F
rlf T1,F
; rlf T0,F ; redundant because of the sign extension
;; Now add it to the last altitude estimate (32 bit add)
;;
movf T3,W ; add temp to altitude
addwf ALTB3,F
movf T2,W
btfsc STATUS,C ; handle the carry
incfsz T2,W
addwf ALTB2,F
movf T1,W
btfsc STATUS,C
incfsz T1,W
addwf ALTB1,F
movf T0,W
btfsc STATUS,C
incfsz T0,W
addwf ALTB0,F
;; Now make a temporary copy of the last acceleration estimate
;; and divide by 128*128*2 or shift right 15 bits. This shift
;; is handled in two parts. The right shift by 16 bits is done in the
;; copying phase just by shifting bytes. Then a 1 bit left shift is performed.
;;
movf ACCELB1,W
movwf T3
movf ACCELB0,W
movwf T2
movlw 0 ; sign extension
btfsc T2,7
movlw 0xff
movwf T1
movwf T0
rlf ACCELB2,W ; pick up the msb of ACCELB2 for the left shift
rlf T3,F
rlf T2,F
; rlf T1,F ; redundant because of the sign extension
;; Now add it into the last altitude estimate (32 bit add)
;;
movf T3,W ; add temp to altitude
addwf ALTB3,F
movf T2,W
btfsc STATUS,C ; handle the carry
incfsz T2,W
addwf ALTB2,F
movf T1,W
btfsc STATUS,C
incfsz T1,W
addwf ALTB1,F
movf T0,W
btfsc STATUS,C
incfsz T0,W
addwf ALTB0,F
;; State update of altitude is now complete
;;
;; Now we update the velocity estimate
;;
;; Make a temporary copy of the last acceleration estimate and
;; multiply by T, which is the same as shifting it right
;; 7 bits
;;
movf ACCELB2,W
movwf T3
movf ACCELB1,W
movwf T2
movf ACCELB0,W
movwf T1
movlw 0 ; sign extension
btfsc T1,7
movlw 0xff
movwf T0
rlf ACCELB3,W ; pick up the msb of VELB3 for the left shift
rlf T3,F
rlf T2,F
rlf T1,F
; rlf T0,F ; redundant because of the sign extension
;; Now add it to the velocity estimate
;;
movf T3,W ; add temp to altitude
addwf VELB3,F
movf T2,W
btfsc STATUS,C ; handle the carry
incfsz T2,W
addwf VELB2,F
movf T1,W
btfsc STATUS,C
incfsz T1,W
addwf VELB1,F
movf T0,W
btfsc STATUS,C
incfsz T0,W
addwf VELB0,F
;; This completes the state update for velocity
;;
;; Acceleration is assumed to be constant so no update of it
;; is required. Move on to the state correction.
;;
;; Begin state correction
;; Call the routine to get the pressure reading. It
;; appears in T0-T3 as a 16.16 fixed point number.
call get_altitude
;; Pressure inovation correction
;;
;; First step is to subtract altitude from pressure reading to
;; get the inovation. Since we need this value for three computations,
;; it is left in T0-T3 for later use. Do not mess with T0-T3!
P_INOVAT:
movf ALTB3,W
subwf T3,F
movf ALTB2,W
btfss STATUS,C
incfsz ALTB2,W
subwf T2,F
movf ALTB1,W
btfss STATUS,C
incfsz ALTB1,W
subwf T1,F
movf ALTB0,W
btfss STATUS,C
incfsz ALTB0,W
subwf T0,F
;; Acceleration correction
;; Multiply the inovation (currently in T0-T3) by the appropriate
;; Kalman gain and add the altitude estimate
;; Since the Kalman gains used are constant powers of two, this is pretty
;; simple.
;; Since the velocity has the highest gain (1/32) it is first.
;;
movlw VELB3
movwf FSR
movlw 5 ; divide T0 by 32
call KALMAC ; Perform the multiply/accumulate
;; Altitude correction. Gain is 1/64 so divide by T0 by 2.
movlw ALTB3
movwf FSR
movlw 1
call KALMAC ; Perform multiply/accumulate
;; Acceleration correction. Gain is the same as altitude.
movlw ACCELB3
movwf FSR
call KALMAC1
;; Keep the get_acceleration call. This just does the ADC conversion
;; so this can be stored to EEPROM
call get_acceleration
;; Fine'
;; Done
;; Compleate
;; That is very fast. The MPLAB simulator indicates about 200 instructions
;; are executed. Which doesn't include the calls to read the sensors.
return
end