I was speaking with a classmate this evening who was having problems understanding what was going on in the measurement linearization function. After an attempt to explain it using the magic of the Identity matrix, I realized the current solution was still overly complex.
This version gets rid of the intermediate Fi matrix and directly inserts the linearized measurements into an Ht of the correct size.
I also took the opportunity to get rid of the extra copy of range_bearing_meas... and the '2' that was hanging around in ekf_slam.m
I was speaking with a classmate this evening who was having problems understanding what was going on in the measurement linearization function. After an attempt to explain it using the magic of the Identity matrix, I realized the current solution was still overly complex.
This version gets rid of the intermediate Fi matrix and directly inserts the linearized measurements into an Ht of the correct size.
I also took the opportunity to get rid of the extra copy of range_bearing_meas... and the '2' that was hanging around in ekf_slam.m