In [ ]:

    

import sys

import numpy as np
import math
from carSimulator import CarSimulator


    

In [ ]:

    

def getControlJacobian(sim, u, x):
    L = sim.L
    B = np.zeros((3,2))
    #...
    
    return B


    

In [ ]:

    

def main():

    sim = CarSimulator()
    u = [.1, .2]  # fixed control signal
    
    # Kalman estimates
    s = np.zeros(3)
    S = np.zeros((3,3))
    
    # initial at true state
    s[0] = sim.x
    s[1] = sim.y
    s[2] = sim.theta
    # with noisy constant
    np.fill_diagonal(S, 0.1)
    
    W = np.identity(4) * sim.observationNoise
    Q = np.identity(3) * sim.dynamicsNoise
    
    A = np.identity(3)
    for t in range(10000):
        sim.step(u)
        y_meassured = sim.getRealNoisyObservation()
        
        # get the linear observation model
        C = sim.getObservationJacobianAtState(s)
        y_mean = sim.getMeanObservationAtState(s)
        
        # get the linear control model
        B = getControlJacobian(sim, u, s)
        a = np.dot(B, u)
        
        # Kalman filter
        # ...
        # Tipp: use variables shat, Shat, c, C and K
        # to compute the new Kalman estimates s and S
        

        # code to display a covariance
        sim.gaussianMeansToDraw = [s[:2]]
        sim.gaussianCovsToDraw =[S[:2, :2]]

        
        # tracking error
        error = np.abs(s - np.array([sim.x, sim.y, sim.theta]))
        print("estim error", np.max(error))
        
        
main()


    

In [ ]: