WPILib Poz Tahmin Edicileri

WPILib, diferansiyel, sapma ve mekanik aktarma organları için poz tahmin ediciler içerir. Bu tahmin ediciler, mevcut:ref:odometry <docs/software/kinematics-and-odometry/index:Kinematics and Odometry> sınıflarının yerine geçecek şekilde tasarlanmıştır ve bir Unscented Kalman Filter ı filtreleyin. Bu tahmin ediciler, kodlayıcı sapmasını ve gürültülü görüntü verilerini hesaba katabilir. Bu tahmin ediciler, sadece update-güncelleme çağrılırsa, karşılık gelen odometri sınıflarıyla aynı şekilde davranabilirler.

Poz tahmin edicileri, durum uzay sistemi kullanarak robot konumunu tahmin eder \(\begin{bmatrix}x & y & \theta \end{bmatrix}^T\), bu robot konumunu Pose2d olarak temsil edebilir . WPILib, robot konumunu tahmin etmek için DifferentialDrivePoseEstimator, SwerveDrivePoseEstimator ve MecanumDrivePoseEstimator içerir. Bunlarda kullanıcılar, robotun tahmini konumunu güncellemek için periyodik olarak kodlayıcı ve cayro ölçümleriyle (odometri sınıflarıyla aynı) update çağırır. Robot, bilgisayar görüşü veya V-SLAM gibi sensörlerden alana göre konumunun (` Pose2d`` olarak kodlanmış) ölçümlerini aldığında, poz tahminci gecikmesi, robot konumunu doğru bir şekilde tahmin etmek için ölçümü telafi eder.

Poz tahmin edicileri, tahmini durum \(\mathbf{\hat{x}}\), hata kovaryansı \(\mathbf{P}\), girdiler ve yerel ölçümler dahil olmak üzere geçmiş gözlemci durumlarının bir listesini depolayarak gecikme telafisi gerçekleştirir. Yeni ölçümler uygulandığında, tahmin edicinin durumu ilk olarak ölçümün zaman damgasına geri döndürülür. Daha sonra filtre, durum tahminini yeni ölçümle düzeltir ve yeni ölçümü dahil etmek için ölçüm zaman damgası ile şimdiki zaman arasındaki girdileri uygular. Bu, aksi takdirde onları kullanılamaz hale getirebilecek ve robot yerelleştirmesi için uygun bir çözüm haline gelebilecek çerçeve hızlarına sahip görüntü işleme çözümlerine olanak tanır.

Aşağıdaki örnek, DifferentialDrivePoseEstimator kullanımını gösterir:

var estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(),
        new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02), // State measurement standard deviations. X, Y, theta.
        new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01), // Local measurement standard deviations. Left encoder, right encoder, gyro.
        new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01)); // Global measurement standard deviations. X, Y, and theta.

Poz Tahmincilerini Ayarlama

Tüm poz tahmin edicileri, model ve ölçümler için kullanıcı tarafından özelleştirilebilir standart sapmalar sunar. Bu standart sapmalar, filtrenin bu durumların her birine ne kadar “güvendiğini” belirler. Örneğin, ölçümler için standart sapmanın arttırılması (gürültülü bir sinyal için yapılabileceği gibi), tahmin edicinin kendi durum tahminine gelen ölçümlerden daha fazla güvenmesine yol açacaktır. Sahada bu, filtrenin model sapmalarını yavaş yavaş düzeltmek pahasına gürültülü görüş verilerini iyi bir şekilde reddedebileceği anlamına gelebilir. Bu değerler önceden tahmin edilebilirken, bunlar büyük ölçüde her robotun benzersiz kurulumuna ve genel ölçüm yöntemine bağlıdır.