EKF реализация

Question

Я пишу EKF для системы, подобной той, что в pi c:

Является ли эта функция правильной? Третья переменная - это модуль скорости, а четвертая - угол, который скорость составляет с северной осью. Я не получаю хорошие результаты для оценки отношения.

void predict(Eigen::Vector4d& x_old,Eigen::Matrix4d F,Eigen::MatrixXd L,Eigen::Matrix4d P,Eigen::Matrix2d Q, double dt){ 

x_new(0)=x_old(0)+(dt*x_old(2)*cos(x_old(3)));
x_new(1)=x_old(1)+(dt*x_old(2)*sin(x_old(3)));
x_new(2)=x_old(2);
x_new(3)=x_old(3);


//F
F<<1, 0, dt*cos(x_old(3)), -dt*x_old(2)*sin(x_old(3)),
  0,1, dt*sin(x_old(3)),dt*x_old(2)*cos(x_old(3)),
  0, 0, 1, 0,
  0, 0, 0, 1;
P_new = F*P*F.transpose() + L*Q*L.transpose();
cout<<"predict"<<endl;
}