Extented kalman filter implementation
1 次查看(过去 30 天)
显示 更早的评论
Hi! I would like to implement the EKF of this system in matlab
the state variable are north and east coordinates, module of velocity, angle with north axis. I tried to write f and F functions. Is this implementation correct? I'm not sure about xnew(3) and xnew(4). I attached the whole code behind. Then..how can I assess the filter performance from a graph? Thanks!
function xnew = f(dT, xold)
xnew = [ xold(1) + dT*xold(3)*cos(xold(4));
xold(2) + dT*xold(3)*sin(xold(4));
xold(3);
atan2(xold(2) + dT*xold(3)*sin(xold(4)), xold(1) + dT*xold(3)*cos(xold(4)));];
end
function jacobian = F(dT, xold)
jacobian = [ 1 0 dT*cos(xold(4)) -dT*xold(3)*sin(xold(4));
0 1 dT*sin(xold(4)) dT*xold(3)*cos(xold(4));
0 0 1 0;
0 0 0 1 ];
end
0 个评论
回答(0 个)
另请参阅
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!