Hello all.
I am doing parameter estimation for a quadcopter drone, and now I want to test the effect of EKF parameter estimation in px4 simulation. To estimate the inertia, I used the angular velocity as the observation and the torque as the input. In the PX4 simulation, I got the angular velocity from the ROS2 topic named “/fmu/out/vehicle_angular_velocity”, and the normalized speed of the motor from the topic named “/fmu/out/actuator_outputs_sim” (I also tried using the “/fmu/out/actuator_motors” topic) , and below is my code for calculating the torque.
Vector4d actuator_sim = inputs.actuator_sim;
Vector4d actuator_cmd = inputs.actuator_cmd;
double l1x = 0.25;
double l1y = 0.15;
double l2x = 0.19;
double l2y = 0.15;
double l3x = 0.25;
double l3y = 0.15;
double l4x = 0.19;
double l4y = 0.15;
actuator_sim = actuator_sim.array() * actuator_sim.array();//use \omega_i^2
auto &omega_1 = actuator_sim(0);
auto &omega_2 = actuator_sim(1);
auto &omega_3 = actuator_sim(2);
auto &omega_4 = actuator_sim(3);
M << (omega_2 * l2x + omega_3 * l3x - omega_1 * l1x - omega_4 * l4x), (omega_1 * l1y + omega_3 * l3y - omega_2 * l2y - omega_4 * l4y),
0.06 * (omega_1 + omega_2 - omega_3 - omega_4);
Because this snippet is just for test, I did not convert the units of the torque to real physical units, but the estimate should converge to a specific value theoretically if eyerything went well.(although not the real inertia).However, the estimates always diverged.
Before doing px4 sitl, I had tested the EKF code for estimating the inertia of a quadcopter drone in matlab (an ideal dynamics model was used for simulation, the controller directly outputs force and torque, and no motor model was used), estimates can be converged very well. I further tested that the matlab code and the code ported to px4 simulation have the same output under the same input, so I think the estimation divergence is due to a problem with the calculation of the torque.