# Robot position calculation - using simplified Filter-Smoother

Yes, this agrees with my estimates. The most counter-intuitive part is Cf.

Friction losses come from the following components:

Force of rolling resistance in floor tiles, tires, and weight bearing (adj) bearings (noun) which is proportional to the mass of the robot:
[INDENT] Frr = Crr1 * Fnormal = Crr1 * mg = Crr * m
[/INDENT]

Force of kinetic friction in drivetrain gear teeth, sprokets, and axles depends on Fnormals which proportional to the transmitted drive force:
[INDENT] Fdtf = Cdtf1 * MotorOutputTorque = Cdft1 * Fdrive / wheel_radius = Cdtf * Fdrive
[/INDENT]

This lets us write Newton’s Law formula as:
[INDENT]ma = (Fdrive - Ffriction) = (Fdrive - Fdtf - Frr)
ma = Fdrive - CdfFdrive - Crr * m
ma = Fdrive * (1-Cdtf) - Crr * m
ma = C1
(Vpwm - Vemf)(1 - Cdtf) - Crrm
ma = C1*(1-Cdtf) * Vpwm - C1*(1-Cdtf)Vemf - Crrm
ma = C1*(1-Cdtf) * Vpwm - C1*(1-Cdtf)C2velocity - Crr*m

a = (C1*(1-Cdtf)/m) * Vpwm - (C1*(1-Cdtf)*C2/m)*velocity - Crr

a = dv/dt = (C3/m) * Vpwm - (C4/m) * v - Crr
[/INDENT]
Solving this differential equation will lead to a familiar formulas:
[INDENT]a(t) = ((Cp/m)Vpwm - Cf) * exp(-t/(mCt))
v(t) = ((Cp/m)Vpwm - Cf) * mCt * ( 1 - exp(-t/(mCt)) )
s(t) = ((Cp/m)Vpwm - Cf) * mCt * ( t - m
Ct (1 - exp(-t/(mCt)) ) )
[/INDENT]
Essentially, Cp and Ct have absorbed Cdtf (kinetic friction coefficients in the drivetrain). While Cf absorbed all rolling resistance coefficients and kinetic friction coefficient between wheel axles and weight bearing bearings (where friction is proportional to the robot mass).

An intuitive explanation to this may be that since Cf signifies power losses then it should not decrease as the robot mass increases.

At the end of the day Cf will roughly stay the same. Finding analytical formulas without any “roughlys” is beyond my expertise in solving partial differential equations so we will depend on filter to smooth out any rough approximations.

And now another tricky question: What will happen to the formulas if we double number of our drivetrain motors while everything else stays the same?

As engineers we could be cavalier about roughness of our coefficients while adding new motors but we will certainly remove some metal to keep robot mass the same.

I would take great care in making this assumption. Basically, this says that the top speed of the robot will be roughly the same if you double the mass. For a 2 lb robot versus a 4 lb robot with 4 motors on the drive train this is going to be a fine assumption. But when comparing a 15 lb robot versus a 30 lb robot with 4 motors on the drive train I wouldn’t expect them to have the same top speed.

I’ll approach this in a similar manner. Firstly, if we ignore friction and look at the acceleration at t=0 we will see that doubling the motors (and thereby doubling the torque) will in turn double the initial acceleration. Therefore Cp should be doubled.

The friction in the drive train will likely increase slightly. Adding more motors often requires additional gears or putting multiple motors on a single axle which can easily begin to bind it due to over constraining the axle. However, if we assume the bot is built decently, these effects will be negligible. Therefore Cf would increase a negligible amount.

Finally we look at Ct. This is an interesting value to figure out. One might immediately think it is doubled or halved, but that can be argued against relatively easy. Let us look at the velocity equation:

It is clear that the top speed of the robot will be reached when time approaches infinity. This reduces the equation to:

The question now is how will the top speed change if the number of motors is doubled?
To answer this I pose a hypothetical scenario. Imagine we have a robot driving at full power and the motors are turning at 70 rpm (assuming 393 torque configurations). If we look at the theoretical torque-speed plots we can see that this implies that the motors are putting out about 4 inch-pounds of torque. Because the robot is driving at top speed, it is not accelerating. This implies that this torque is due solely to friction. If we double the motors we effectively half the torque load on each individual motor. Looking at the chart we see that a torque load of 2 inch-pounds implies a speed of about 85 rpm.
So from this we can deduce that the top speed has increased. With a little work we can figure that the amount the top speed increase is a function of what the old top speed was. Doubling the motors would triple your top speed if it was initially 20 rpm but it wouldn’t even double if your initial motor speed was 40 rpm.
In reality, VEX drive train motors can’t run for any long duration of time below 65-ish rpm at full power without overheating. As such, we can see how doubling our motors would affect our top speed if our initial speeds were 70, 80, and 90 rpm. These yield top speed multipliers of approximately 1.21, 1.14, and 1.06 respectively. So the end conclusion is that the top speed will increase, but definitely not double.

We have already said that Cp will double and Cf will stay about the same. Therefore, if we want our top speed to increase but not double, then the Ct value must decrease but be above half of its initial value. This makes sense because it implies that the robot will reach top speed quicker by doubling the number of motors.

To conclude, if the number of motors on the drive train is doubled:
Cp will double.
Cf may increase a negligible amount.
Ct will decrease, but be above half its initial value (the value depends on the old top speed of the robot and by how much Cp dominated Cf)

It should be noted that my Ct derivation assumed that the friction torque at top speed is the same for the robot before and after doubling motors. This should be a fair assumption since the speed of the robot is changing much.

Tricky!
Again, I hope I didn’t do anything silly.

Yes, they will not have the same top speed:
[INDENT]v(inf) = (CpCtVpwm - CfCtm)
[/INDENT]
So even with Cf staying the same we are going to have twice as much power (velocity) losses at the end.

As for the power, once you mentioned torque-speed plots, I start doubting my original assumptions. I am going to check my math to see if the solution already accounts for non-lineriarity of power curve or I made an error somewhere along the way.

You’re right, I missed that.

As far as the non-linearity of the power curve is concerned, this should be covered by your inclusion of back-emf as a function of motor speed. As the motor speed increases, the back emf does as well. This reduces your effective voltage and as such your torque. You modeled this such that the torque output decreases linearly with motor speed which fits with the known response of the VEX motors.

Looking back I see there is an underlying assumption that the inductance of the motor is very small compared to the resistance of the motor. This is a fine assumption to make in this case and it allows you to fit each of the transient response equations onto a single line. If you didn’t, the acceleration formula would look like this (the variables are different motor parameters).

I hope I am not bogging you down in the details as I am definitely excited to see where the next step takes your project. I just enjoy discussing the intricacies of these types of things when others have the time to do so.

Ok, I finally verified my math and it needs a couple of corrections. But before I post the latest formulas here is an interesting thought experiment:

Let say you had a robot and you found values for Cp, Cf, and Ct that define its a(t), v(t), and s(t) curves. Then we double the mass of the robot and get new curves.

Then we double the number of motors in the drivetrain while keeping mass the same (double the original). What would be new a, v, s curves?

Now imagine that instead of single robot with (2m) mass and (2n) motors we have split it into two virtual robots each having original mass (m) and number of motors (n).

Wouldn’t both of the robots move exactly as our original robot with the same formulas for a(t), v(t), and s(t)?

The correct answer is that they almost will, but there is one more important parameter that needs to be considered.

This is just a container post to hold attachments. Full description is on the next page.

I tried to avoid long formulas with large number of greek letters but, I guess, there is no way around - it has to be done.

Let us consider our simple robot of mass (m) that drives forward using (n) motors each set to the same power. Important difference from previous page is that now we account for drivetrain components having combined moment of inertia (Idt) as measured around wheel axles. To spin them up we would need torque (Tdt) which we incorporate into net force equation as a force (Fdt) in the floor plane by dividing torque with the drive wheels’ radius (rw). Similarly drive wheels’ angular acceleration and velocities are expressed via robot’s forward going velocity and acceleration.

(Tdrive) is a total torque generated by (n) motors that is proportional to some coefficient (Cdt) which converts voltage differential between motors’ input voltage (Vpwm) and (Vemf) into available torque. Note that (Cdt) will account for kinetic friction losses inside the motors which are proportional to the torque provided by the motors.

(Frrf) denotes friction force loss from rolling resistance in the soft floor tiles and wheel tires. Since (Frrf) is proportional to the robot’s mass via Fnormal=mg it will be expressed as coefficient (Crrf)*(m). Finally, (Cemf) is a coefficient that relates (Vemf) with wheel’s angular velocity (w).

As you can see robot’s acceleration (a), velocity (v), and driven distance (s) could be expressed using three coefficients (Cp), (Cf), and (Ct) which could be found experimentally. Since we know how those coefficients relate to robot’s mass, number of motors and driving wheel radius, if necessary, we could derive underlying coefficients (Cdt), (Crrf), and (Cemf) as well.

So how do we determine value of coefficients (Cp), (Cf), and (Ct). Turns out it is easy. All you need is to conduct two experiments with your robot.

The first experiment is to place the robot on some supports such that wheels rotate freely in the air and let it run. Then after wheels have reached stable speed you log quad encoder measurements at several time points.

The second experiment is very similar, but you let the robot run on the floor tiles so that you can observe rolling resistance coefficient (Cf).

Here is a graphical illustration of those experiments:

https://vexforum.com/attachment.php?attachmentid=9665

Horizontal axis is time and vertical is for velocity (v) and distance (s). Distance plots (blue lines) will approach grey lines, whose slope correspond to the final velocities of wheels in each experiment. The point where those lines intersect horizontal axis corresponds to (Ct) components due to either (Idt) or robot mass (m).

Note: the formula on the graph reads s(infinity)=(…)(t-Ct) it is really supposed to mean “large value of t” when exponential portion practically goes to zero.

In the next example our robot will only rotate in place. We assume either a two-wheeled turtle-bot or a symmetrical x-drive where each wheel is at a distance (rr) from the robot’s center of rotation. It is also assumed that center of rotation roughly coincides with the center of mass (otherwise equations will become crazy complex).

All (n) motors will be run with the same power (Vpwm) in the same direction to cause robot rotation (i.e. clockwise or anti-clockwise).

Robot’s moment of inertia around its center of rotation is (Ir) and drivetrain monent of inertia around wheels’ axles is (Idt).

Assuming there is no wheel slippage robot’s body angular acceleration and velocity could be expressed with wheels’ angular acceleration (alpha) and velocity (w).

First we write the force equation. Robot’s body will resist angular acceleration with torque (Tr) which translates to force (Fr=Tr/rr) at the wheel touch points. Similarly, drivetrain will resist accelration with (Fdt=Tdt/rw).

Solutions for (w) and (theta), which are wheels’ angular velocity and total rotation angle can be easily converted to angular velocity and heading of the robot’s body.

https://vexforum.com/attachment.php?attachmentid=9671

Resulting formulas depend on the same (Cp) and (Cf) as for forward motion but the value of timing coefficient (Ct2) is different from (Ct) because angular motion depends on the robot’s moment of inertia (Ir). To find (Ct2) you use the same technique of running robot on the ground (rotating) and then plotting quad encoder measurements vs time.

As I was reviewing my math solving for both translation and rotation of the robot - there were a couple of questions that came up.

First, the easy question:

Every motor has some viscous friction which is proportional to the rotation speed and reduces available torque by (Tvf = Cvf*w). I don’t mention it explicitly in the above equations, because experimentally measured value for Cemf conveniently accounts for both viscous friction and electromotive force.

We only talk about kinetic friction which is proportional to the output torque and rolling resistance friction which is proportional to the slope of the indentation robot makes in the tiles and therefore it mass. But there must be something else, because when wheels are in the air, none of those two forces are present, yet when power is turned off wheels will lose their energy and stop.

In the lab settings Cvf would be easy to measure by monitoring energy flow into the motor that runs without load. However, few of us have access to a good test equipment.

So here is an easy puzzle/question: find one or more easy ways to measure approximate Cvf value of a VEX motor using nothing but common VEX parts.

The second question is more complex. It might be easy for people who do mechanical dynamics for living, but I am not one of them.

Suppose we have X-drive robot with omni-wheels. Motors for both wheels in each diagonal pair will be commanded the same power, but each pair may have different level (no robot body rotation). Also, assuming kinetic friction in the omni rollers is negligible.

Then it should be possible to apply forward motion equations (from post#41) to each wheel pair independently. In the robot’s frame of reference one pair will give solution for dx and another for dy. This lets you draw the path of the robot as a function of time. Even though the path may be curved due to non-linear acceleration, dx and dy are fully independent, because of the X-drive geometry.

Is this correct? I will appreciate if somebody with experience weights in.

Your calculations are very nice and verbose. The only thing that strikes me as odd is that the force due to rolling friction doesn’t appear to be a function of the robots velocity.

I wouldn’t consider myself experienced, but I can take a shot at it.
Based on how you have described the example, your conclusion should be correct. The principle is similar to the classic projectile motion problem where the kinematics in the x and y directions are completely uncoupled and can be written independent of one another.

I can only think of one caveat. I imagine that the energy lost from the robot due to the interaction between the wheel and the soft foam floor is a function of the wheels velocity. Even with friction-less rollers, energy is still lost as the wheels translate sideways across the field and has to push the “wave” of foam around. If this is the case, then the problem turns into something similar to projectile motion with drag and the two equations become coupled. However, this effect probably doesn’t become dominant until the robot gets quite heavy and starts to significantly sink into the field so I think it would be fair to ignore it and go with the previous conclusion so long as the robot isn’t too heavy.

Just to clarify, the last part was referring to the kinetics of the problem presented (which would come up if one was trying to work through the prediction step equations like you have been). The kinematics remain uncoupled.

My knowledge of the rolling resistance is limited to the wikipedia article and a few other sources I could find on the internet. Most research is done about soft inflated tires running on the hard surface. The dominant factors determining Crr in that case are tire temperature and pressure with vehicle velocity taking the third place and can actually slightly decrease with higher velocities before going up again in some scenarios (see fig 2.23 on pg16).

In cases where tires are firm, the dominant opinion is to use constant Crr and that velocity doesn’t need to be considered at all.

We have slightly different situation with firm tires running on somewhat soft surface. Few sources that consider rolling resistance on the soft surfaces don’t even talk about velocity in that context. The best source on this subject analyzes aircrafts running on the grass and soil surfaces and is hardly applicable to our scenario.

So, here is how I understand it after looking through the above links:

https://vexforum.com/attachment.php?attachmentid=9679

When robot is at rest it makes an indentation in the foam surface with the depth somehow proportional to its weight. The section of the foam-to-wheel contact is depicted by (ℓ).

When robot goes forward with speed (Vrobot) it starts climbing out of the hole. As it moves toward the edge the new section of the foam will starts compressing and will go down with some speed (Vsink). The faster the robot moves - the closer it gets to the edge, and the smaller is the new section of the floor (ℓ) that now supports robot’s weight. The smaller that section is the larger will be the pressure and the faster the foam will sink.

For each value of velocity (Vrobot) there will be a point of equilibrium when foam sinks down just as fast as the robot advances. If we measure value of Crr it will correspond to a slope at an angle (theta) such that it takes the robot to advance distance (delta s) just as fast as the foam sinks to the depth (delta h). Without having analytical models, the simplest way to determine relation between Vrobot and Vsink increases is, essentially, by running an experiment to determine the value of Crr at various robot velocities.

If I could persuade technik jr that I really need chassis finished to run this experiment, and we have chassis done … then I’ll try it. But it will, probably, be much longer before chassis are properly aligned and my measurement task is fully debugged to get enough sensitivity for a conclusive Crr vs velocity plot.

Edit: almost forgot to mention the most important part: (Cf) coefficient that includes (Crr) is the only coefficient that does not fully depend on the robot as it might change from field to field. So the smart thing to do is to let filter estimate this coefficient as part of its state. The robot will start with hardcoded average value for (Cf) then drive a couple of feet straight forward and estimate correction for (Cf) on this specific field. By the time robot starts making turns and needs more precision it will already have refined value of (Cf). If it is, indeed, a case that (Crr) considerably depends on the speed, then we will see the pattern in the drifting filter estimates of (Cf) at various speeds and will be able to easily fix equations.

Even though we didn’t have enough progress on our testing rig to post any data, technik jr had a good question about equations. What would happen if (Vpwm=0)? Will robot drive backwards because of (-Cf)?

Obviously, it will not. The short answer is that friction is a resistive force. It exists only when external force is applied, resisting object’s movement. If there is no external force then friction force is zero.

https://sp.yimg.com/ib/th?id=OIP.M3d9337db6596c5c6308e9deafb503a26H0&pid=15.1&rs=1&c=1&qlt=95&w=146&h=118

For the longer answer we need to look at the image in the previous post. On the left figure the wheel is at rest. Foam that it touches pushes it up vertically and in horizontal direction equally opposing forces push it to the center from both left and right sides.

The right figure shows moving wheel in the state of dynamic equilibrium. Notice that while it interacts with the foam on the right, it is not touching any on the left. The horizontal component of the force is asymmetrical and pushes it only to the left. If you imagine robot suddenly losing all its forward velocity (and all kinetic energy as a result) it would actually roll back to the left for a little bit, while the floor rebounds on the left, sinks on the right, and eventually forms symmetrical supports on both sides.

So in case of rolling resistance friction robot will actually drive backwards while its support is asymmetric. Since foam of the field rebounds and sinks very fast it will not take a lot of time.

while I must admit, this thread has totally lost me in it’s advanced state, I have been able to successfully implement a kalman filter on my robot, it only takes in data from two sources (but from one of them in two different ways)

the goal of this filter is to get a more accurate gyro reading that doesn’t drift as much over time

first input Is that raw gyro value. Then I use past gyro values to predicted angular acceleration and velocity for the next phase, and I use this to calculate where I predict my angle to be for the next phase, the idea here is to prevent drift, I then use a kalman filter to combine these values.

Next I compare values from the left and right encoders to predict a change In angle during that timestep, and I combine this with my gyro value’s in a second kalman filter.

Testing remains to be done to see if the filtered version is more accurate than the unfiltered one, but I hope it is

I think these codes are very useful, but not very precise in the vex robotics level.

Sorry, to hear that. I thought that when technik jr lost me after the first differential equation, there was still some room to go with the older students. But I understand you - it is a difficult subject.

That’s great! Our initial goal was to use information from various noisy sources to reduce overall error and it looks like it works for you!

Prediction of the future sensor measurements is the most important first step towards setting up a filter that is capable of doing sensor fusion. You definitely have a functioning filter, however I wouldn’t call it Kalman Filter. Formal definition of KF requires it to do specific thing with the state uncertainty (also called covariance or think about it as error bounds). You can still go some way toward Kalman Filter by initially hardcoding how much you trust each of the sources: current predicted state, gyro reading, or quad encoders on the wheels.

I think that, instead of using two independent filters, having one filter that fuses both sources will be much better option. Two independent filters just smooth the noise each in its own channel. A single filter that combines all data sources will be able to take advantage of the cross-correlations. Your second filter sounds like it might be the one, but from your description I couldn’t tell for sure.

Collin, could you share your code? I feel this thread might be better served if we start from something that already works and most readers understand, rather than trying to rewrite a text book on kinematics of rigid bodies or theory of estimation.

I must admit that I don’t have enough access to Cortex and parts to conduct my own experiments. I promised technik jr and teammates that this season I will not touch the robot until they have first fully functioning prototype. They choose to interpret that in the most literal way.

Good news is that robot already drives around and picks up the balls. Even better news is that they have learned how friction affects torque as it travels through the gear-train. They actually used formulas to design the latest flywheel setup. It used to be that it would need two force motors to start, but now after they have done more realistic torque estimates and aligned bearings to reduce friction, a single speed motor could start and spun up to speed 5” flywheel with 21:1 external gear ratio!

The bottom line is: I will really appreciate if people with “full access” to functioning robots will help drive this thread and keep it on the easy-to-understand level.

maybe I am lost as to what a kalman filter is, but I used this site as a tutorial and then modified the prediction state to work with prediction of future reading.

http://home.wlu.edu/~levys/kalman_tutorial/

this tutorial says we would use PastState*A to predict the next state, but the state of my gyro would not change based on a constant multiplied by the last state, so I needed a more complex prediction.

here is the code, I want to preface this by saying “working” means it compiled without error, I have not yet been able to test if this filter effectively works to calculate my next state with more accuracy than just a gyro. we have been re-building our robot recently so I don’t have total access to it right now. I have a little code bot I use that I will bring home tomorrow to test the code on

float gain =  0;
float R = .016; // published accuracy of the gyro
float lastAngle = theta;// theta is the value coming from my gyro
float lastDAngle = 0;// the change in angle from the last cycle
float aAngle = 0; // angular acceleration
float thetaP = theta; // prediction for the next state of theat
float Perr = 0; // prediction error
float lastPerr = 0;// prediction error from last cycle
float DangleP = 0;// predicted change in angle for last cycle
float Dangle = 0; // change in angle during this cycle
float A, AE;// a constant that represents a constant for which lastState * A = Current state
float thetaPE = theta;// predicted next angle using encoder
float PerrE = 0;// prediction error from encoder
float lastPerrE = 0;
float gainE = 0; //encoder gain
float DanglePE = 0;// predicted change in angle based on encoders
float ldist, sdist, rad;// larger wheel distance, smaller wheel distance, radius of turn
float circum;// circumference of circle robot is moving on
float Eerr = (8/360);// published error of encoder
////////////////////////////////////////////////////////////////////////////////////////////
while(true){
// PREDICTION PHASE 1 - using Gyro
///////////////////////////////
Dangle = theta - lastAngle;
aAngle = Dangle - lastDAngle;
DangleP = lastDAngle+aAngle;
thetaP = lastAngle + DangleP;
A = thetaP/lastAngle;
Perr = A*lastPerr*A;
// PREDICTION PHASE 2 - using encoder
/////////////////////////////////////////////
if(abs(LD) > abs(RD)){
ldist = -LD;
sdist =  RD;
}
if(abs(RD) > abs(LD)){
ldist = RD;
sdist = LD;
}
DanglePE = ldist/(circum+.001);
thetaPE = lastAngle + DanglePE;
AE = thetaPE/lastAngle;
PerrE = AE * lastPerrE*AE;

//UPDATE PHASE 1 using Gyro and Predicted Gyro
/////////////////////////////////////////////
gain = Perr/(Perr+R);
thetaP = thetaP + gain*(theta - thetaP);
Perr = (1-gain)*Perr;
// thetaP now becomes the excepted value for theta, we will now combine this with a third input
// which is the encoders, to gain a third prediction of what theta should be
// by combining the three predictions we hope to arrive at a more accurate conclusion
////////////////////////////////////
////////////////////////////////////

//UPDATE PHASE 2 - using thetaP and predicted angle based on wheel encoders
//////////////////////////////////////////////////////////////////////////
gainE = PerrE/(PerrE + Eerr);
thetaPE = thetaPE + gainE*(thetaP-thetaPE);
PerrE = (1-gainE)*PerrE;
//////////////////////////////////////////////////////////////////

// Now ThetaPE, which is our prediction combinging, the measured Gyro Value
// the predicted gyro value based on past gyro values
// and the predicted gyro value based on the encoders
// becomes the accepted value for our current angle

//SETTING PHASE - here we set our angle and sensors to all the calculated values, and
// set the recursive variable so that they can be used in the next loop

SensorValue[gyro1] = SensorValue[gyro] = thetaPE*10;
lastAngle = thetaPE;
lastDAngle = Dangle;
lastPerr = Perr;
lastPerrE = PerrE;

}

}

That’s a great link! It could save us lots of posts. Everybody who wants to learn about filters, please, read it to the end. It has lots of math symbols when it gets to matrices, but just try to read it - descriptions are very good.

Now back to your code, Collin. At first I was confused by the variable names, but once I figured out what was going on I was very impressed by how much filter you have implemented! There are several problems though.

My best guess is that you followed tutorial through the scalar parts and correctly implemented formulas for scalar filter. You tried to use two scalar filters. First one to filter out drift of the gyroscope and the second to combine gyro and encoder measurements.

The first problem is that Mr. Levy introduced process noise (Q) for computing predicted value of § only in part 14, well into matrix section of the tutorial. As a result you are ending up with Perr and PerrE both having values of zero. This leads to gainE being zero and, essentially, resulting thetaPE depending only on quad encoders and ignoring gyroscope.

It would be easy to fix by adding some process noise: (Perr = A * Perr * A + Q). Where value for Q is found by trial and error for each type of sensor. However, I am still trying to wrap my mind around how the values for Perr and PerrE would change over time.

The second problem is sensor fusion. If you somehow get those two independent filters do the fusion, it would be simply amazing. But most likely painful and you will end up with the code equivalent to a single multi-parameter filter.

It may sound as if in order to go multi-parameter you must to use matrices and vectors. That is not true. For just three sensors the code may actually end up shorter without them. And if you need to be more advanced, there is an excellent BNSLib implementing vectors, matrices, and lots of other cool stuff ([https://github.com/JMarple/BNSLibrary).

Finally, I don’t see any delay() in your loop, so the task must be cycling fast doing updates as soon as the sensor values change one tick at a time. I am most concerned about implicit assumptions about the time between measurements. Everytime another task interrupts this task (taking some time) you will end up with an error trying to predict future values based on the deltas.

The fix for that would be to timestamp every measurement and use time explicitly in your predict and update steps.

To help transition to that mode I will include code for my sensor task. It is not tested yet, but we will improve it and learn how to tie it into the filter code in the following posts.](https://github.com/JMarple/BNSLibrary).)

Disclaimer: this code for high fidelity measurement task is still untested at the time of writing. Once edit window for this post closes, an updated version could be found at https://github.com/technik3k/VexLib3k.

Notes:

According to RobotC help page on available data types long is 64 bit, while float is only 32 bit. If we were going to keep timestamp in floats with microsecond (10^-6) precision we would only be able to reference 32 second time periods at most. Going to 64-bit longs has no such problem.

We intend to start sensor task with kHighPriority and further [;
for( i=0 ; i<hf_sensor_cnt ; i++ )
{
curVal* = sensorValue[hf_sensor_id[i]];
curCnt* = -1;
}
hogCPU(); // obtain exclusive lock on CPU by this task
long nSysCurr = nSysTime;
while(nSysCurr == nSysTime); // spin until timer ticks
nSysCurr = nSysTime;
for( n=0 ; 1 ; n++ )
{
if( nSysCurr == nSysTime ) // still in the same time tick
{
for( i=0 ; i<hf_sensor_cnt ; i++ )
{
short s = sensorValue[hf_sensor_id[i]];
if( curVal* != s ) // has sensor changed?
{
curVal* = s; // remember new value
curCnt* = n; // remember sub-tick count
}
}
}
else // nSysTime changed
{
int min_dt = 101000;
for(i=0;i<hf_sensor_cnt;i++)
{
if( curCnt
>= 0 ) // there was sensor change during prev tick cycle
{
hf_sensor_val* = curVal*;
hf_sensor_t* = nSysCurr * 1000 + (curCnt**1000)/n;
curCnt* = -2; // this sensor has been read in this measurement cycle
}
else if( curCnt* == -1 ) // for sensors that still haven’t changed
{
long dt = hf_sensor_nxt* - nSysTime * 1000; // when would it change?
if( dt>0 && dt < min_dt)
{
min_dt = dt; // remember closest future expected change time
}
}
}
if( min_dt > 3000 ) // if it is more than 3 msec away - yield to other tasks
{
break;
}
nSysCurr = nSysTime; // otherwise monitor next clock cycle
n = 0;
}
}
releaseCPU(); // release exclusive lock on CPU by this task
delay(1); // yield at least one 1 msec cycle to other tasks
}
}
[/CODE]

In the next post I will try to show how to do filter predict and update steps based on timestamps, but if somebody wants to jump ahead and post it first, please, feel free to do so.*************](“http://help.robotc.net/WebHelpVEX/index.htm#Resources/topics/VEX_Cortex/ROBOTC/Task_Control/hogCPU.htm”)

Lets do our first filter example. It is going to be very simple. In fact, Collin’s code is far ahead of it, because he uses covariance (Perr, PerrE). This example will not use covariance and will trust measurements 100%, essentially having gain = 1.

Consider a simple contraption, like this jpearman’s gyro test rig:

Alternatively, you may mount Cortex onto the turntable gear, put battery on top of it and attach gyro to the battery to address issues discussed in this thread. One way or another have gyro that could stay still or be rotated when you choose so.

We are going to use the following variables (names are consistent with Dr. Levy’s tutorial):

long Tk1; // timestamp of the gyro measurement (internal unit is usec)
short Zk; // gyro measurement (internal unit is gyro ticks)
long Tk;  // time of the current state estimate (internal unit is usec)
float Xk[4]; // state estimates at a time Tk:
// Xk[0] - gyro bias (how much gyro had drifted from heading) (ticks)
// Xk[1] - gyro drift rate (how much gyro drifts per unit of time)(ticks/usec)
// Xk[2] - heading (of the robot or platform that gyro is mounted on) (ticks)
// Xk[3] - angular velocity (rotation rate of robot or platform) (ticks/usec)

Predict step will move state Xk at a time Tk to the time of the measurement Tk1:

float dt = 1.0*(Tk1-Tk);
Xk[0] = Xk[0] + Xk[1]*dt;   // expected gyro bias
Xk[2] = Xk[2] + Xk[3]*dt;   // expected platform heading
Tk = Tk1;

Next the measurement update step will compute residuals and corrections to state estimates:

float Yk = Zk - (Xk[2] + Xk[0]); // residual = actual reading - expected value

if( motor_power == 0 ) // we are expecting that gyro platform is stationary
{
Xk[0] = Xk[0] + ( 1.0    ) * Yk;      // updating gyro bias
Xk[1] = Xk[1] + ( 1.0/dt ) * Yk;      // and drift estimates
}
else // motor turning the platform
{
// if( residual_ratio > some_max ) // robot cannot physically turn this fast
//   do something to handle this abnormal situation
//   for example, reject this measurement and panic if it's 5th time
Xk[2] = Xk[2] + ( 1.0    ) * Yk;      // update robot heading
Xk[3] = Xk[3] + ( 1.0/dt ) * Yk;      // and angular velocity estimates
}

We assume that previous gyro update was at time Tk and it had been drifting for the period of (dt).

Finally, we do some housekeeping and predict next measurement time for the precision sensor_task:

long nextTk=Tk+1.0/(Xk[1]+Xk[3]); // t = s/v = 1 tick/(internal+platform drifts)

To understand state updates, ask yourself this question: what if actual measurement came (n) ticks over the expected value? What does it mean and what needs to be done?

If platform is stationary then we need to update our gyro bias and increase modeled drift rate such that next time expected and actual readings will match up. (Collin, you, essentially, do that in your code when you reset sensorValues] back to zero or thetaPE).

If the platform is in motion (motor power applied) then we assume that drift stays the same and just count gyro changes toward the platform motion. Any drift errors during that time will introduce errors to our final solution, but there is nothing we can do about that with just one sensor. You need to be careful to extend “powered” filter mode some time beyond actual motor power is turned off, since robot still moves using up its kinetic energy.

Believe it or not, but since we have 4 states and just one measurement source, we are using matrix form of the equations and partial derivatives. If you look at tutorial part 19 or EKF on wikipedia you will see the equation for Xk measurement update is:

[INDENT]Xk = Xk + Kk * Yk
Xk = Xk + Gk (Zk - h(Xk))[/INDENT]
Where Xk is a vector of state estimates, Yk is a vector of measurement residuals (Yk=Zk-h(Xk)) and the only confusing part is that tutorial calls gain matrix Gk, while wikipedia and everybody else call it Kk.

In our example Xk is a vector of 4 values, Yk is a vector of 1 mesurement, anf Gk is a matrix {1.0, 1.0/dt, 0, 0} for stationary mode and { 0, 0, 1.0, 1.0/dt } for “powered” mode.

So we have successfully figured out partials without using any differential equations or anything else equally complex that you would expect from the name.