This is a complete re-working of the answer I had originally provided. If you're curious, you can check the edit history and see what was posted earlier.
In comments to this question, OP stated that they might be able to get throttle and steering angles for the robot, but they probably wouldn't be accurate. That's okay; it's better than nothing.
OP also stated that the IMU outputs a fused orientation, where the fused orientation is from the accelerometer, gyro, and magnetometer.
The accelerometer output of the IMU may or may not be corrected by the orientation. If it is, great. If not, it's a straightforward conversion provided you pick the correct form; I think generally what you would want is the XYZ Tait-Bryan rotation matrix, but definitely check with the manufacturer and absolutely run through copious amounts of test data to check the results yourself.
Side note here, I build quick visualizers for myself all the time and highly recommend you do it for yourself, too. Here's a quick Matlab snippet to use, assuming you have a variable time that has all of your time indices. If you just have the sample intervals, you can make time as the cumulative sum of the sample intervals; time = cumsum(sampleIntervals);.
width = 1;
length = 2;
height = 0.5;
shapeTop = [...
-width/2, -length/2; ...
-width/2, length/2; ...
width/2, length/2; ...
width/2, -length/2];
shapeTop(:,3) = ones(4,1)*height/2;
shapeBottom = shapeTop;
shapeBottom(:,3) = shapeBottom(:,3) - ones(4,1)*height;
shapeVertices = [shapeTop;shapeBottom];
shapeFaces = [...
1,2,3,4,1; ...
5,6,7,8,5; ...
8,5,1,4,8; ...
7,6,2,3,7; ...
5,6,2,1,5; ...
8,7,3,4,8];
shapeColor = [0.6 0.6 1]; % Light blue
figure(1)
shapePatch = patch(...
'Faces', shapeFaces, ...
'Vertices', shapeVertices, ...
'FaceColor', shapeColor);
axis equal;
xlabel('X-Axis')
ylabel('Y-Axis')
zlabel('Z-Axis')
view([60,20]);
tic
while true
elapsedTime = toc;
if elapsedTime > time(end)
break;
end
currentSample = find(time>=elapsedTime,1);
rotMatrix = EulerToRotationMatrix(...
EulerX(currentSample) , ...
EulerY(currentSample) , ...
EulerZ(currentSample));
tempVertices = shapeVertices;
tempVertices = (rotMatrix*tempVertices.').';
set(shapePatch,'Vertices',tempVertices);
drawnow;
pause(0.01);
end
This code runs through all of your sample data, at a 1:1 playback speed, and updates a shape's orientation according to your X/Y/Z angles. EulerToRotationMatrix is a function you make yourself that accepts your Euler x/y/z angles and returns the appropriate rotation matrix. That's the part you need to play around with, but again I'm pretty sure you want Tait-Bryan XYZ.
Record multiple test data sets of you moving your IMU from one starting pose to another starting pose, and take different paths to end at the same orientation. When you watch the playback you'll see pretty much immediately if you're doing the rotation matrix stuff correctly.
So anyways, once you're sure you've got your rotation matrix, if you need to correct the accelerometer data, it's just accNew = rotMatrix*accOld;.
Now, there are four possible ways to update yaw information:
- From a model using inputs (throttle and steering angle).
- Magnetometer for angle relative to magnetic North.
- Gyro readings from IMU
- Heading required to move in a straight line from the previous GPS fix to the current GPS fix.
The GPS location is a Gaussian distribution about your actual location, so I would be very reluctant to use option 4 above for anything.
The IMU already fuses the magnetometer and gyro data into one yaw reading, so then the only thing that you can do to improve your yaw estimate is with the model. If you can't get access to the inputs (throttle/steering angle) then you're stuck with the yaw angle the IMU gives you.
If you do get access to the inputs, then you can model the x/y/$\theta$ rates as follows:
$$
\dot{x} = v\cos(\frac{\pi}{2} - \theta) \\
\dot{y} = v\sin(\frac{\pi}{2} - \theta) \\
\dot{\theta} = \frac{v}{L}\tan(\delta) \\
$$
where $\theta$ is the heading, measured positive CW from the +y-axis; $\delta$ is the steering angle, measured positive CW from the forward vehicle direction, $L$ is the wheel base, the distance between the front and rear axles, and $v$ is the speed of the vehicle.
The model of how your vehicle parameters (position/heading) update are nonlinear to the point that I wouldn't try to put it in state space form. You really do need the actual $\cos$ and $\sin$ functions; the small-angle approximations wouldn't really work here. That's okay! You're using the extended Kalman filter, so you don't need to try to linearize the model.
I think I'd probably try to model the throttle signal as a first-order speed regulator, such that:
$$
\dot{v} = \frac{c\left(\mbox{throttle}\right) - v}{\tau} \\
$$
where $\tau$ is the time constant and $c$ is a value that scales the throttle to a speed. Start a test with the vehicle stationary, then give a step command for throttle. The long-term, stable speed can be used to determine $c$. Then you can figure, given a particular speed "request" (throttle position), what the time constant was.
The relationship between step input and system response is well known; 63.2% of final value at one time constant, 95.0% at three time constants, etc. I'd run multiple tests at each of a variety of throttle inputs and average the results.
Now, your more complete model for the system would be:
$$
\dot{v} = \frac{c\left(\mbox{throttle}\right) - v}{\tau} \\
v = v + \dot{v}\Delta T \\
$$
$$
\dot{x} = v\cos(\frac{\pi}{2} - \theta) \\
\dot{y} = v\sin(\frac{\pi}{2} - \theta) \\
\dot{\theta} = \frac{v}{L}\tan(\delta) \\
$$
$$
x = x + \dot{x}dT \\
y = y + \dot{y}dT \\
\theta = \theta + \dot{\theta}dT \\
$$
The generic Kalman model.
You're using the extended Kalman filter which, unlike the regular ("classic"?) Kalman filter, doesn't require a linear system. This is great because the system model is right above. Your states are position, speed, and yaw angle.
So you do your predict steps:
- Predict the state estimate:
$$
\hat{x}_{t|t-1} = f\left(\hat{x}_{t-1} , u_{t-1}\right) \\
$$
Here $\hat{x}_{t|t-1}$ is your prediction of the state vector $x$, which is what the hat $\hat{ }$ means, for the current time step given the state vector at the previous time step, which is what the ${t|t-1}$ means.
- Predict the error covariance estimate:
$$
P_{t|t-1} = F_k P_{t-1} F_k^T + Q_k \\
$$
Here $F_k$ is the Jacobian $\left.\frac{\partial f}{\partial x}\right|_{\hat{x}_{t-1},u_{t-1}}$, $P$ is the error covariance (how much you "trust" the state estimate; smaller = more faith), $Q_k$ is the process noise matrix, which is typically a diagonal matrix of [numbers]. How do you measure process noise? There's a bunch of papers on the subject and you should check them out. Try putting 0.1 on the diagonal and see how that works. Generally the process noise is how you tune the filter. Again, the $t$ and $t|t-1$ mean the same as they did (current and current given the past).
Then you do the update steps:
Here's the tricky-ish thing for you. You have multiple sensors - a GPS and an IMU. What to do? If you had independent sensors, the answer would be update, then update again. For example, if you were using the IMU only for speed and yaw angle, and you were using the GPS only for position, then you would have the following residuals/innovations:
$$
\tilde{y}_{\mbox{IMU}} = \left[\begin{matrix}{}
\mbox{speed measurement} \\
\mbox{yaw angle measurement}
\end{matrix} \right] - \left[\begin{matrix}{}
0 & 1 & 0 \\
0 & 0 & 1
\end{matrix} \right] \left[\begin{matrix}{}
\mbox{position state prediction} \\
\mbox{speed state prediction} \\
\mbox{yaw angle state prediction}
\end{matrix} \right]
$$
$$
\tilde{y}_{\mbox{GPS}} = \left[\begin{matrix}{}
\mbox{position measurement}
\end{matrix} \right] - \left[\begin{matrix}{}
1 & 0 & 0
\end{matrix} \right] \left[\begin{matrix}{}
\mbox{position state prediction} \\
\mbox{speed state prediction} \\
\mbox{yaw angle state prediction}
\end{matrix} \right]
$$
You can then go on calculating the residual covariances and Kalman gains without worry because those measurements are independent and don't "overwrite" each other.
You're not so lucky; you have to deal with the fact that you've got two redundant measurements - IMU position and GPS position. The common way to handle the redundancy is to average the sensors before you send them to the filter. This works fine for two identical sensors, but that's not what you have.
A better way might be to weight the measurements by their covariances such that more "trustworthy" sensors get a bigger weight.
All that said, the two don't have to conflict; the IMU technically outputs an acceleration. If you just integrate one time to get speed and leave it at that, then suddenly the IMU and the GPS don't conflict and you can leave their measurements separate and not have to worry about conflict. This is the route I would take. If you find it unsatisfactory you can try integrating the IMU output again and blending it with the GPS data, but I don't think that's going to make things any better.
Hopefully I've made things much more clear.
A final note, you'll probably find also that your GPS and IMU don't update at the same rate. What to do then? There's a lot of papers about that, but they basically boil down to downsampling the higher frequency data or faking the low frequency data. Downsampling can be done dumb (discarding data) or smart (filtering data), just like faking the data can be done dumb (repeating the same value) or smart (some manner of extrapolating a projected sensor reading).
