Add error computation for PF, improve least square fit