SUBROUTINE Vel2Accel(VLast,AcLast,JkLast,Varray,AcArray) c Subroutine to calculate 40 accelerations from velocities c by differencing each velocity sample. Use VLast as starting c point; AcLast to ride through bad velocity at any point C If a vel sample is the same as last, calc accel change (jerk) C from 2 prev & 2 next samples, or just prev jerk if near C beginning or end of array. Also detect couplets that avg C to close to the linear fit - these are artifact errors C Common access is only for 'Init' IMPLICIT none INCLUDE "FastCom.ftni" C Passed Parameters REAL VLast,AcLast,JkLast,Varray(40),AcArray(40) C Local storage INTEGER i, NumGood REAL Diff1,Diff2,Diff3 REAL VTemp(40),LTemp,ATemp,JTemp,AcTemp(40) REAL Slope, Offset REAL S_XiYi, S_Xi, S_Yi, S_Xi_2, RGood, RTmp LOGICAL Corrected C Diag for print if change Corrected = .FALSE. ! always start with original LTemp = VLast ! entry stuff if needed ATemp = AcLast JTemp = JkLast DO i = 1,40 ! do all samples this sec VTemp(i) = Varray(i) ! save for possible diag IF (VLast.LT.-900.) THEN ! no prev vel AcArray(i) = AcLast ! just ride through with same accel JkLast = 0. ! no change in Accel, so jerk=0 VLast = Varray(i) ! save new one; hopefully next will be good ELSEIF (Varray(i).LT.-900.) THEN ! current Vel is bad AcArray(i) = AcLast ! again, ride through JkLast = 0. VLast = Varray(i) ! unfortunately, next will also be bad ELSE ! both last & current are good IF (Varray(i).EQ.VLast) THEN ! repeated velocity C if we are in the middle, try calc'ing difference in vel for last & next IF (i.GT.2.AND.i.LT.39 .AND. ! in the middle & Varray(i-2).GT.-900. .AND. Varray(i-1).GT.-900. .AND. ! all samples are good & Varray(i+1).GT.-900. .AND. Varray(i+2).GT.-900.) THEN Diff1 = Varray(i-1) - Varray(i-2) ! prev accel Diff2 = Varray(i+2) - Varray(i+1) ! next accel Diff3 = (Diff2 - Diff1)/3. + Diff1 ! accel from V(i-1) to v(i) Varray(i) = Varray(i-1) + Diff3 ELSE ! too close to an end point Diff1 = AcLast + JkLast/40. ! expected new accel, in G's Varray(i) = VLast + Diff1/(40. * 0.10197) ! correct to meters/sec ENDIF Corrected = .TRUE. ! show we changed something ENDIF ! done handling a duplicate velocity C Handle actual vel->accel calc AcArray(i) = (Varray(i) - VLast) * 40. * 0.10197 ! m/s^2 -> G's JkLast = AcArray(i) - AcLast ! calc new jerk AcLast = AcArray(i) ! save for next VLast = Varray(i) ENDIF IF (i.EQ.2 .AND. Init) THEN ! time jump or not flying AcArray(1) = AcArray(2) ! VLast may have been old JkLast = 0. ! backfilling Accel means no jerk ENDIF c save Accel before couplet detect AcTemp(i) = AcArray(i) END DO ! end of loop on 40 samples C for couplet detection, first calc linear fit C Do the initial summations NumGood = 0 ! start w/no good data points S_XiYi = 0. ! getting ready for sums ! This is Sum(XiYi) S_Xi = 0. ! Sum(Xi) ! Xi's are the Freqs S_Yi = 0. ! Sum(Yi) ! Yi's are the tb's S_Xi_2 = 0. ! Sum(Xi^2) DO i = 1,40 ! loop through all data points IF (AcArray(i).GT.-900.) THEN ! only sum in the acceptable ones NumGood = NumGood + 1 ! bump count of good samples S_XiYi = S_XiYi + i*AcArray(i) S_Xi = S_Xi + i S_Yi = S_Yi + AcArray(i) S_Xi_2 = S_Xi_2 + i*i ENDIF END DO RGood = Float(NumGood) ! easier to do once C Do the Slope Calc Slope = RGood*S_XiYi - S_Xi*S_Yi ! Numerator nS(XiYi)-S(Xi)S(Yi) RTmp = RGood*S_Xi_2 - S_Xi*S_Xi ! Denominator nS(Xi^2)-S(Xi)^2 IF (RTmp.NE.0.) THEN ! guard against /0 Slope = Slope/RTmp ELSE Slope = 0. ! dummy value ENDIF C Do the offset calc Offset = (S_Yi-Slope*S_Xi)/RGood c Loop through, trying to find couplets Corrected = .FALSE. ! assume no changes DO i = 1,39 IF (Abs(AcArray(i)-Slope*i-Offset).GT.0.15 .AND. ! 1st loc is bad & Abs(AcArray(i+1)-Slope*(i+1)-Offset).GT.0.15 .AND. ! 2nd loc is bad & Abs(AcArray(i)-AcArray(i+1)).GT.0.3 .AND. ! big difference between them & Abs( (AcArray(i)+AcArray(i+1))/2. ! but avg of the 2 & -Slope*(i+.5)-Offset).LT.0.2 ) THEN ! is close to the line AcArray(i) = Slope*i + Offset ! fix both AcArray(i+1) = Slope*(i+1) + Offset Corrected = .TRUE. ! and flag that we made a change ENDIF END DO IF (Corrected) THEN ! recalc the linear fit NumGood = 0 ! start w/no good data points S_XiYi = 0. ! getting ready for sums ! This is Sum(XiYi) S_Xi = 0. ! Sum(Xi) ! Xi's are the Freqs S_Yi = 0. ! Sum(Yi) ! Yi's are the tb's S_Xi_2 = 0. ! Sum(Xi^2) DO i = 1,40 ! loop through all data points IF (AcArray(i).GT.-900.) THEN ! only sum in the acceptable ones NumGood = NumGood + 1 ! bump count of good samples S_XiYi = S_XiYi + i*AcArray(i) S_Xi = S_Xi + i S_Yi = S_Yi + AcArray(i) S_Xi_2 = S_Xi_2 + i*i ENDIF END DO RGood = Float(NumGood) ! easier to do once C Do the Slope Calc Slope = RGood*S_XiYi - S_Xi*S_Yi ! Numerator nS(XiYi)-S(Xi)S(Yi) RTmp = RGood*S_Xi_2 - S_Xi*S_Xi ! Denominator nS(Xi^2)-S(Xi)^2 IF (RTmp.NE.0.) THEN ! guard against /0 Slope = Slope/RTmp ELSE Slope = 0. ! dummy value ENDIF C Do the offset calc Offset = (S_Yi-Slope*S_Xi)/RGood ENDIF ! done w/recalc if necessary C Test 1st & last points - these won't show in couplet test IF (Abs(AcArray(1)-Slope*1-Offset).GT.0.15) ! 1st loc is bad & AcArray(1) = Slope*1 + Offset IF (Abs(AcArray(40)-Slope*40-Offset).GT.0.15) ! 1st loc is bad & AcArray(40) = Slope*40 + Offset C diag print c WRITE(*,'("VLast:"f8.3" AcLast:"f8.3" JkLast:"f8.3)') c & LTemp,ATemp,JTemp c WRITE(*,'("VOrig:"10f8.3)') (VTemp(i),i=1,10) c WRITE(*,'(3(6x,10f8.3,/))') (VTemp(i),i=11,40) c WRITE(*,'(" VNew:"10f8.3)') (Varray(i),i=1,10) c WRITE(*,'(3(6x,10f8.3,/))') (Varray(i),i=11,40) c WRITE(*,'("AOrig:"10f8.3)') (AcTemp(i),i=1,10) c WRITE(*,'(3(6x,10f8.3,/))') (AcTemp(i),i=11,40) c WRITE(*,'(" ANew:"10f8.3)') (AcArray(i),i=1,10) c WRITE(*,'(3(6x,10f8.3,/))') (AcArray(i),i=11,40) RETURN END