diff --git a/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m b/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m new file mode 100644 index 0000000000..bfb0bc6108 --- /dev/null +++ b/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m @@ -0,0 +1,8 @@ +clear all; +syms spd yaw real; +syms R_spd R_yaw real; +vx = spd*cos(yaw); +vy = spd*sin(yaw); +Tpc = jacobian([vx;vy],[spd;yaw]); +R_polar = [R_spd 0;0 R_yaw]; +R_cartesian = Tpc*R_polar*Tpc'; \ No newline at end of file