User Tools
Differences
This shows you the differences between two versions of the page.
| Both sides previous revision Previous revision | Last revision Both sides next revision | ||
|
cs:localization:kalman:algorithm:start [2017/01/21 19:27] Brian Moore |
cs:localization:kalman:algorithm:start [2017/01/21 19:29] Brian Moore |
||
|---|---|---|---|
| Line 243: | Line 243: | ||
| == Kalman Update Loop == | == Kalman Update Loop == | ||
| <code matlab> | <code matlab> | ||
| - | %% Run Kalman Filter | + | %% Run Kalman Filter |
| %Estimate new state from last state estimate | %Estimate new state from last state estimate | ||
| X_est(:,k+1) = A*X_est(:,k) + B*(rr(X_est(3,k))*motor*u(:,k)); | X_est(:,k+1) = A*X_est(:,k) + B*(rr(X_est(3,k))*motor*u(:,k)); | ||
| Line 252: | Line 252: | ||
| %Sensors only check in every n seconds | %Sensors only check in every n seconds | ||
| if((mod(k*dt,0.5)) == 0) | if((mod(k*dt,0.5)) == 0) | ||
| - | %Calculate Kalman Gain | + | %Calculate Kalman Gain |
| - | K = (P(:,:,k+1)*C')/(C*P(:,:,k+1)*C' + diag(sigma.^2)); | + | K = (P(:,:,k+1)*C')/(C*P(:,:,k+1)*C' + diag(sigma.^2)); |
| - | + | ||
| - | %Resolve new estimate | + | %Resolve new estimate |
| - | X_est(:,k+1) = X_est(:,k+1)+K*(Y(:,k+1) - C*X_est(:,k+1)); | + | X_est(:,k+1) = X_est(:,k+1)+K*(Y(:,k+1) - C*X_est(:,k+1)); |
| - | %Resolve new covariance matrix for estimate | + | %Resolve new covariance matrix for estimate |
| - | P(:,:,k+1) = (eye(size(X_est,1))-K*C)*P(:,:,k+1); | + | P(:,:,k+1) = (eye(size(X_est,1))-K*C)*P(:,:,k+1); |
| end | end | ||
| | | ||
