Kalman State Update with Control

Problem

Perform one Kalman state update with an observation matrix h and a control term. Given prior mean, prior variance, observation, measurement variance r, control coefficient b, and control input u, return the updated mean prior + K * (obs - h * prior) + b * u, where K = p_prior * h / (h^2 * p_prior + r).

Return the scalar updated state.

Hint

Apply the measurement correction first, then add the control contribution b * u.

Test cases

case 1

in: [0,1,2,1,1,0.5,3]

expected: 2.5

case 2

in: [5,2,7,1,0.5,0,0]

expected: 6.6

case 3

in: [3,1,3,2,2,1.5,2]

expected: 5

case 4

in: [1,0.5,4,1,0,0.2,1]

expected: 4.2

Starter codePython

def kalman_state_update_control(prior, p_prior, obs, h, r, b, u):
    # Your code here
    pass

4 test cases · run your code against them in the editor