#NEWLINE# def kf_likelihood_g( self , F , L , Qc , H , R , Pinf , dF , dQc , dPinf , dR , X , Y ) : #NEWLINE# #INDENT# n = F . shape [ 0 ] #NEWLINE# steps = Y . shape [ 1 ] #NEWLINE# nparam = dF . shape [ 2 ] #NEWLINE# t = X . squeeze ( ) #NEWLINE# e = 0 #NEWLINE# eg = np . zeros ( nparam ) #NEWLINE# m = np . zeros ( [ n , 1 ] ) #NEWLINE# P = Pinf . copy ( ) #NEWLINE# dm = np . zeros ( [ n , nparam ] ) #NEWLINE# dP = dPinf . copy ( ) #NEWLINE# mm = m . copy ( ) #NEWLINE# PP = P . copy ( ) #NEWLINE# dt = ( - np . Inf ) #NEWLINE# AA = np . zeros ( [ ( 2 * n ) , ( 2 * n ) , nparam ] ) #NEWLINE# FF = np . zeros ( [ ( 2 * n ) , ( 2 * n ) ] ) #NEWLINE# for k in range ( 0 , steps ) : #NEWLINE# #INDENT# dt_old = dt #NEWLINE# if ( k > 0 ) : #NEWLINE# #INDENT# dt = ( t [ k ] - t [ ( k - 1 ) ] ) #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# dt = 0 #NEWLINE# #UNINDENT# for j in range ( 0 , nparam ) : #NEWLINE# #INDENT# if ( abs ( ( dt - dt_old ) ) > 1e-09 ) : #NEWLINE# #INDENT# FF [ : n , : n ] = F #NEWLINE# FF [ n : , : n ] = dF [ : , : , j ] #NEWLINE# FF [ n : , n : ] = F #NEWLINE# AA [ : , : , j ] = linalg . expm3 ( ( FF * dt ) ) #NEWLINE# #UNINDENT# foo = AA [ : , : , j ] . dot ( np . vstack ( [ m , dm [ : , j : ( j + 1 ) ] ] ) ) #NEWLINE# mm = foo [ : n , : ] #NEWLINE# dm [ : , j : ( j + 1 ) ] = foo [ n : , : ] #NEWLINE# if ( j == 0 ) : #NEWLINE# #INDENT# A = AA [ : n , : n , j ] #NEWLINE# Q = ( Pinf - A . dot ( Pinf ) . dot ( A . T ) ) #NEWLINE# PP = ( A . dot ( P ) . dot ( A . T ) + Q ) #NEWLINE# #UNINDENT# dA = AA [ n : , : n , j ] #NEWLINE# dQ = ( ( ( dPinf [ : , : , j ] - dA . dot ( Pinf ) . dot ( A . T ) ) - A . dot ( dPinf [ : , : , j ] ) . dot ( A . T ) ) - A . dot ( Pinf ) . dot ( dA . T ) ) #NEWLINE# dP [ : , : , j ] = ( ( ( dA . dot ( P ) . dot ( A . T ) + A . dot ( dP [ : , : , j ] ) . dot ( A . T ) ) + A . dot ( P ) . dot ( dA . T ) ) + dQ ) #NEWLINE# #UNINDENT# m = mm #NEWLINE# P = PP #NEWLINE# S = ( H . dot ( P ) . dot ( H . T ) + R ) #NEWLINE# HtiS = ( H . T / S ) #NEWLINE# iS = ( 1 / S ) #NEWLINE# K = P . dot ( HtiS ) #NEWLINE# v = ( Y [ : , k ] - H . dot ( m ) ) #NEWLINE# vtiS = ( v . T / S ) #NEWLINE# for j in range ( 0 , nparam ) : #NEWLINE# #INDENT# dS = ( H . dot ( dP [ : , : , j ] ) . dot ( H . T ) + dR [ : , : , j ] ) #NEWLINE# eg [ j ] = ( ( ( ( eg [ j ] - ( 0.5 * np . sum ( ( iS * dS ) ) ) ) + ( 0.5 * H . dot ( dm [ : , j : ( j + 1 ) ] ) . dot ( vtiS . T ) ) ) + ( 0.5 * vtiS . dot ( dS ) . dot ( vtiS . T ) ) ) + ( 0.5 * vtiS . dot ( H . dot ( dm [ : , j : ( j + 1 ) ] ) ) ) ) #NEWLINE# dK = ( dP [ : , : , j ] . dot ( HtiS ) - ( P . dot ( HtiS ) . dot ( dS ) / S ) ) #NEWLINE# dm [ : , j : ( j + 1 ) ] = ( ( dm [ : , j : ( j + 1 ) ] + dK . dot ( v ) ) - K . dot ( H ) . dot ( dm [ : , j : ( j + 1 ) ] ) ) #NEWLINE# dKSKt = dK . dot ( S ) . dot ( K . T ) #NEWLINE# dP [ : , : , j ] = ( ( ( dP [ : , : , j ] - dKSKt ) - K . dot ( dS ) . dot ( K . T ) ) - dKSKt . T ) #NEWLINE# #UNINDENT# e = ( ( ( e - ( ( 0.5 * S . shape [ 0 ] ) * np . log ( ( 2 * np . pi ) ) ) ) - np . sum ( np . log ( np . sqrt ( S ) ) ) ) - ( 0.5 * vtiS . dot ( v ) ) ) #NEWLINE# m = ( m + K . dot ( v ) ) #NEWLINE# P = ( P - K . dot ( S ) . dot ( K . T ) ) #NEWLINE# P = ( ( P + P . T ) / 2 ) #NEWLINE# dP = ( ( dP + dP . transpose ( [ 1 , 0 , 2 ] ) ) / 2 ) #NEWLINE# #UNINDENT# return eg