Fix & clean up EKF/UKF implementations

[regexident]

Issues addressed in this PR:

In Kalman Filters

• Q is a square matrix with a size N × N, with N equal to the dimensions of the state vector.
• R is a square matrix with a size M × M, with M equal to the dimensions of the output vector.

extended_kalman_filter.py

Covariance matrices

In the EKF implementation …

• the state vector has 4 dimensions
• the output vector has 2 dimensions

As such Qsim would be expected to be of size 4 × 4, yet was found to be of size 2 × 2.

Noise

Further more the noise was not applied correctly:

• Q is not to be applied to u, but to x, as it is the state (i.e. x) noise covariance
• R is not to be applied to x, but to z, as it is the state (i.e. z) noise covariance

unscented_kalman_filter.py

Covariance matrices

In the UKF implementation …

• the state vector has 4 dimensions
• the output vector has 2 dimensions

As such Qsim would be expected to be of size 4 × 4, yet was found to be of size 2 × 2.

Further more Qsim and Rsim seem to have been mixed up: The angle is a relevant part of the state not the output.

Noise

Further more the noise was not applied correctly:

• R is not to be applied to u, but to z, as it is the state (i.e. z) noise covariance

def observation(xTrue, xd, u):

In both cases the implementation of def observation(…): seems invalid to me.

It should be:

def observation(xTrue, xd, u):

    xd = motion_model(xd, u)
    xTrue = motion_model(xTrue, u) + Qsim @ np.random.randn(4, 1)
    z = observation_model(xTrue) + Rsim @ np.random.randn(2, 1)

    return xTrue, xd, z

See the formulas from Example application, technical on Wikipedia, specifically:

Where …

xTrue = motion_model(xTrue, u) + Qsim @ np.random.randn(4, 1)

… implements …

… while …

z = observation_model(xTrue) + Rsim @ np.random.randn(2, 1)

… implements …

… while …

xd = motion_model(xd, u)

… assumes lack of noise, resulting in massive drifts for dead-reckoning.

---

The reason this code was incorrect, yet produced convincing results is that the dead-reckoning was actually fed noisy inputs, while EKF/UKF worked with mostly clean measurements.

[coveralls]

Pull Request Test Coverage Report for Build 843

• 30 of 30 (100.0%) changed or added relevant lines in 2 files are covered.
• 11 unchanged lines in 4 files lost coverage.
• Overall coverage increased (+0.2%) to 90.571%

---

Files with Coverage Reduction	New Missed Lines	%
PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py	1	80.61%
PathPlanning/LQRRRTStar/lqr_rrt_star.py	2	84.78%
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py	3	90.77%
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py	5	81.79%

Totals
Change from base Build 842:	0.2%
Covered Lines:	6599
Relevant Lines:	7286

---

💛 - Coveralls

[AtsushiSakai]

@regexident Hi. Thank you for your PR. Your code is very clean. I like it.

[AtsushiSakai]

@regexident However, I cannot understand some part of it. Please check the review.

[regexident]

@atsushi-sakai sure!

(I'm not seeing a review though, just two top-level comments of yours.
Did you maybe forget to submit it?)

[AtsushiSakai]

@regexident please check this.

[AtsushiSakai]

Why you add noise to xTrue? xTrue is the true state without noise.

[regexident]

But the system within which the "robot" operates is noisy, no?
You would never find a vehicle be able to move on a "perfectly" circular path in a real environment. There will always be tiny perturbations.

The dead-reckoning however works on the theoretical and thus clean inputs alone, and hence diverges from the true state over time.

It's the "clean x" state, that's wrong, not the noisy one (at least in regards to the process noise, it should be reasonably clean from observation noise).

[AtsushiSakai]

In my assumption, xTrue is the true robot state without noise, which is a circle trajectory. however dead-reckoning sensor has noise, so xd is a trajectory with dead-reckoning with noise.

[regexident]

With real-life effects like wheel slippage, carpet "springiness" and uneven floors a robot is unlikely to ever drive a perfect circle, as the one seen in these demos.

Note that the "effect of an unknown input" from this …

(note that there is no term since we have no known control inputs. Instead, we assume that is the effect of an unknown input and applies that effect to the state vector) …

… also applies to scenarios with a known control input, referred to as process noise (i.e. Qsim).

So as long as Qsim is anything but a zero-filled matrix you will get a true state that's not following a perfect path (as you'd get by dead-reckoning its pure inputs).

---

Additionally the current version of the code feeds the EKF/UKF with (noisy) x, y coordinate measurements, while feeding the DR with (noise-added) yawrate, velocity inputs, which is somewhat akin to comparing apple and oranges. If you wanted a "perfect circle" for xTrue you would need to keep Qsim at (near-)zero and probably change the measurements for both to be yawrate, velocity by changing these:

def observation_model(x):
    #  Observation Model
    H = np.array([
        [0, 0, 1, 0], // yaw rate
        [0, 0, 0, 1] // velocity
    ])
    z = H @ x
    return z

def observation(x, u):
    xTrue = motion_model(x, u) + Qsim @ np.random.randn(4, 1)
    z = observation_model(x) + Rsim @ np.random.randn(2, 1)

    uDR = u + Rsim @ np.random.randn(2, 1)
    xDR = motion_model(xDR, uDR)

    return xTrue, xDR, z

At which point you would get a perfect circle for xTrue and a noticeable drift for xDR.

The problem I would see with this however is that it would somewhat violate your goal 1.):

1. Easy to read for understanding each algorithm's basic idea.

… by obfuscating the relationships between x, u, z, Q and R in Kalman Filters. As you're effectively making use of the fact that you input u is of the same structure as your output z, hence allowing you to perform u + Rsim @ np.random.randn(2, 1), which in the general case you cannot (due to likely differences in dimensionality and structure).

In contrast: the code in this PR expresses those relationships clearly and has them match the formulas found in text-books.

[AtsushiSakai]

With real-life effects like wheel slippage, carpet "springiness" and uneven floors a robot is unlikely to ever drive a perfect circle, as the one seen in these demos.

I know that but I think it is not needed, because the purpose of this code is to understand the EKF algorithm, not the behavior of a robot in a real situation..

[AtsushiSakai]

Why did you remove ud? ud is the input with noise.

[regexident]

Because previously Qsim/Rsim were wrongly applied to u, instead of x/z, resulting in ud.

This observation is also backed by the example given on Wikipedia as well as several other articles on KFs.

[AtsushiSakai]

hmm... I think it is impossible to use u in a real situation, because every sensor for input (like wheel encoder and IMU) has noise.

[AtsushiSakai]

As I said, I like your clean code. But I cannot understand your point about the change related to the algorithm..

[AtsushiSakai]

With real-life effects like wheel slippage, carpet "springiness" and uneven floors a robot is unlikely to ever drive a perfect circle, as the one seen in these demos.

I know that but I think it is not needed, because the purpose of this code is to understand the EKF algorithm, not the behavior of a robot in a real situation..

[regexident]

With real-life effects like wheel slippage, carpet "springiness" and uneven floors a robot is unlikely to ever drive a perfect circle, as the one seen in these demos.

I know that but I think it is not needed, because the purpose of this code is to understand the EKF algorithm, not the behavior of a robot in a real situation..

I did not so much change the algorithm, but rather correct it. The current version of def observation(xTrue, xd, u): is incorrect, from what I can see.

This is supported by the fact, that the current implementations of EKF and UKF mix up Qsim and Rsim:

# extended_kalman_filter.py

# Estimation parameter of EKF
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # ✅
R = np.diag([1.0, 1.0])**2 # ✅

#  Simulation parameter
# ❌ this line seems incorrect: `z` is of structure `x, y`. No angles, so why the `deg2rad`?
Qsim = np.diag([1.0, np.deg2rad(30.0)])**2
Rsim = np.diag([0.5, 0.5])**2 # ✅

def observation(xTrue, xd, u):

    # ❌ this is missing `… + Qsim @ np.random.randn(4, 1)` (see Wikipedia):
    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    # ❌ this seems incorrect:
    # 1. `observation_model(xTrue)` should be used here to obtain `z`. Even if just to be explicit and clear.
    # 2. this is missing `… + Rsim @ np.random.randn(2, 1)` (see Wikipedia)
    # 3. `Rsim` is a `4 × 4` matrix, which is incompatible to `z`, a `2 × 1` vector
    # 4. instead of doing element-wise algebra this is clearer expressed matrix-vector operations
    zx = xTrue[0, 0] + np.random.randn() * Rsim[0, 0]
    # ❌ same as line above
    zy = xTrue[1, 0] + np.random.randn() * Rsim[1, 1]
    # ❌ the `.T` transformation here is misleading and avoidable if expressed with matrix-vector operations.
    z = np.array([[zx, zy]]).T

    # add noise to input
    # ❌ this seems incorrect:
    # 1. `u` should not receive any noise. `x` should, instead (see Wikipedia)
    # 2. `Qsim` is a `2 × 2` matrix, and `z`, a `2 × 1` vector, so by coincidence they are compatible.
    # 3. In the general case `Qsim` and `z` are incompatible though, which may confuse an uneducated reader.
    # 4. the first value of `u` is an angle, but in `Qsim` only the second element corresponds to an angle.
    # 5. instead of doing element-wise algebra this is clearer expressed matrix-vector operations
    ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0]
    # ❌ same as line above
    ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1]
    # ❌ the `.T` transformation here is misleading and avoidable if expressed with matrix-vector operations.
    ud = np.array([[ud1, ud2]]).T

    xd = motion_model(xd, ud)

    return xTrue, z, xd, ud

# unscented_kalman_filter.py

# Estimation parameter of UKF
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # ✅
# ❌ this line seems incorrect: `z` is of structure `x, y`. No angles, so why the `deg2rad`?
R = np.diag([1.0, np.deg2rad(40.0)])**2

#  Simulation parameter
# ❌ this line seems incorrect: `x` is a `4 × 1` vector, as such `Q` should be a `4 × 4` matrix
Qsim = np.diag([0.5, 0.5])**2
# ❌ this line seems incorrect: `z` is of structure `x, y`. No angles, so why the `deg2rad`?
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2 # ⚠️

def observation(xTrue, xd, u):

    # ❌ same issues as commented for EKF version
    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    # ❌ same issues as commented for EKF version, plus it's mixing up `Qsim` and `Rsim`
    zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
    # ❌ same as line above
    zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
    # ❌ same issues as commented for EKF version
    z = np.array([[zx, zy]]).T

    # add noise to input
    # ❌ same issues as commented for EKF version, plus it's mixing up `Qsim` and `Rsim`
    ud1 = u[0] + np.random.randn() * Rsim[0, 0]
    # ❌ same as line above
    ud2 = u[1] + np.random.randn() * Rsim[1, 1]
    ud = np.array([ud1, ud2])

    xd = motion_model(xd, ud)

    return xTrue, z, xd, ud

Try modelling the original "version" of the algorithm with pure matrix-vector operations and you will see that they are not compatible dimension-wise, which supports my claim that the current algorithm is incorrect.

[regexident]

If you want to have a "nice and clean" true path
Then the code should have a zero-matrix for Qsim and Q. And the measurement z should be yawrate, velocity, not x, y, so that you can feed it into the DR and the EKF. Right now you're feeding them different data, hence not really making a comparison between them, imho.

Qsim = np.diag([
    0.00001, # variance of location on x-axis
    0.00001, # variance of location on y-axis
    0.00001, # variance of yaw-rate
    0.00001, # variance of velocity
])**2

def jacobH(x):
    jH = <calculate jacobian for new observation model>
    return jH

def observation_model(x):
    H = np.array([
        [0, 0, 1, 0], # yaw-rate
        [0, 0, 0, 1], # velocity
    ])
    z = H @ x
    return z

# replacement for `def observation(xTrue, xDR, u):`
def model(x, u):
    x = motion_model(x, u) + Qsim @ np.random.randn(4, 1)
    z = observation_model(x) + Rsim @ np.random.randn(2, 1)
    return x, z

def main():
    # ...
    while SIM_TIME >= time:
        # ...
        xTrue, z = model(xTrue, u)
        
        xEst, PEst = ekf_estimation(xEst, PEst, z, u)

        # This is abusing the fact that `u` and `z` are of equal structure,
        # thus allowing one to pass a measurement (i.e. `z`) as input (i.e. `u`).
        # While this works in this particular instance of a KF, it will most likely lead to confusion.
        xDR = motion_model(xDR, z)
        # ...

But again, I'd argue that this wouldn't really be beneficial to giving a clear picture of how Kalman Filters work, generalizably.

[AtsushiSakai]

@regexident OK. Thank you for your instructions. I will fix it up by myself later. I appreciate your interest : ).

[regexident]

👍🏻 🙇🏻‍♂️