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