fix traj cost computation

acxz · Nov 23, 2019 · 98a7c5b · 98a7c5b
1 parent 822bb7c
commit 98a7c5b
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/mppi.m b/mppi.m
@@ -121,7 +121,7 @@
         % Forward propagation
         x_traj(:,:,timestep_num+1) = func_F(x_traj(:,:,timestep_num),func_g(v_traj(:,:,timestep_num)),dt);
 
-        traj_cost = traj_cost + func_run_cost(x_traj(:,:,timestep_num+1)) + learning_rate * (sample_u_traj(:,timestep_num)' * (inv(ctrl_noise_covar) * v_traj(:,:,timestep_num)));
+        traj_cost = traj_cost + func_run_cost(x_traj(:,:,timestep_num+1)) + learning_rate * sample_u_traj(:,timestep_num)' * inv(ctrl_noise_covar) * (sample_u_traj(:,timestep_num) - v_traj(:,:,timestep_num));
 
         if(print_verbose)
           fprintf("TN: %d, IN: %d, DU: %d, Simtime: %d\n", timestep_num, iteration, mean(sum(abs(du),1)), time);