from numpy import *

mi=matrix([[0.],[0.]]) #initial state
sigma=matrix([[1000., 0.],[0., 1000]]) #initial uncertainty
#u=matrix([[0.],[0.]]) #ext. motion
A=matrix([[1., 1.],[0., 1.]]) #motion funct
C=matrix([[1. ,0.]]) #meas. funct.
Q=matrix([[1.]]) #meas. uncert.
R=matrix([[1., 0.],[0., 1.]]) #Mov uncert.
I=matrix([[1., 0.],[0., 1.]]) #Identity

def KF_observ(mi, sigma, z):
        z=matrix([[z]])
        K=sigma*C.T*(C*sigma*C.T+Q).I
        mi=mi+(K*(z -(C*mi)))
        sigma=(I-(K*C))*sigma

        return [mi, sigma]

def KF_est(mi, sigma):
        mi=(A*mi)
        sigma=(A*sigma*A.T) + R
        
        return [mi, sigma]
         
#[mi, sigma] = KF_est(mi, sigma)
#[mi, sigma] = KF_observ(mi, sigma, 1)
#print mi
#print sigma