I'm working on a vehicle control and using a 9DOF sensor (accelerometer, magnetometer and gyroscope). For the yaw angle, I have a discontinuity problem at pi rad. (180 deg.).
I'm controlling the vehicle with a PID controller and when the vehicle turns more than 180 deg, the sign suddenly changes (from 180 to -180) and this makes the controller act weird. The same problem will occur when it turns more than -180 deg too.
As the method, I'm using a direction cosine matrix to calculate euler angles. (recommended method for the sparkfun sensor.)
My question is what kind of approach should I use? How to deal with this discontinuity in the case of using a PID controller to control yaw angle.
I had the same problem and I'm doing the following:
#define MOD(a) ((a > 180.0) ? (a - 360.0) : ((a < -180.0) ? (a + 360.0) : a))
The difference of 2 angles can simply be put back to -180d/+180d with such formula.
Is this your problem?
After desired angle changes it's sign(ie. 180->-180)
then suddenly control input(ie. P * error) becomes bigger.
For example.
--------------------------------
| desired | real | error |
|-----------|--------|---------|
| 170 | 160 | 10 |
| -170 | 160 | -330 |
--------------------------------
If your problem is same as I understood, how about this?
Before we go, note that It is assumed that every angle values are in range of [-pi,pi].
error = desired - real;
if(error > 180)
error = error - 360;
else if(error < -180)
error = error + 360;
else
error = error;//do nothing
This method always chooses a direction such that control input becomes smaller.(You know there are two choices for direction)
Related
This is the script that I have. It works till the ------ separation. Under I do not get any error from Matlab, but neither do I get a return of bestDx nor bestDy. Please help. (The first part is given just to put you in context)
%%
% Variables after running script Read_eA3_file.m
%date_time_UTC
%reflectivity
%clutter_mask
%Convert units
dBZ = reflectivity * 0.375 - 30;
dBZ_Mask = clutter_mask * 0.375 - 30;
%Replace clutter values with NaN
weather = NaN(size(dBZ)); %initialise to constant
weather(dBZ>=dBZ_Mask) = dBZ(dBZ>=dBZ_Mask); %copy values when A >= B
%Reduce to range -- those are 384x384 arrays
dBZ_range = dBZ(:,:,1:16); %16:18 to 16:23 included
weather_range = weather(:,:,1:16); %16:18 to 16:23 included
weather1618 = weather(:,:,1); %16:18 map only
weather1623 = weather(:,:,16); %16:23 map only
% Plot maps
image(imrotate(-weather1618,90)); %of 16:18
image(imrotate(-weather1623,90)); %of 16:23
%Find x,y of strongest dBZ
%Since the value are all negative. I look for their minimun
[M,I] = min(weather1618(:)); %for 16:18
[I_row, I_col] = ind2sub(size(weather1618),I); %values are 255 and 143
[M2,I2] = min(weather1623(:)); %for 16:23
[I2_row, I2_col] = ind2sub(size(weather1623),I2); %values are 223 and 7
%Calc displacement
%I get a value of 139.7140
max_displ=sqrt((I2_row-I_row)^2+(I2_col-I_col)^2); %between 1618 and 1623
%%
% -----Section below does not work; ONLY RUN the section ABOVE---------
%% Find Dx Dy for max_corr between two maps
maxCoeff=0;
weather1618Modified = zeros(384,384); %create weather array for time range
%weather1618Modified(:) = {NaN}; % Matlab cannot mix cell & double
%%
for x = 1:384
for y = 1:384
%30 pixel appx.
for Dx = -max_displ:30: max_displ
for Dy = -max_displ:30: max_displ
%Limit range of x+Dx and y+Dy to 1:384
if x+Dx<1 | y+Dy<1 | x+Dx>384 | y+Dy>384
continue
%weather1618Modified is the forecasted weather1823
weather1618Modified(x+Dx,y+Dy) = weather1618(x,y)
%Find the best correlation; Is corrcoef the right formula?
newCoeff=corrcoef(weather1623,weather1618Modified);
if newCoeff>maxCoeff
maxCoeff=newCoeff;
bestDx=Dx;
bestDy=Dy;
end
end
end
end
end
end
%% Calc displacement
bestDispl = sqrt(bestDx^2+bestDy^2); %bestDispl for a 5 min frame
%Calc speed
speed = bestDispl/time;
You have to delete the continue statement after the first if (or place it somewhere else).
The continue statement makes the program skip the remaining part of the for-loop and go directly to the next iteration. Therefore bestDx and bestDy will never be set.
Documentation: https://se.mathworks.com/help/matlab/ref/continue.html
Given the following Bayesian network determine the probabilities.
On the network shown in Figure 1, suppose that:
P("alternator broken"=true) = 0.02
P("no charging"=true | "alternator broken"=true) = 0.95
P("no charging"=true | "alternator broken"=false) = 0.01.
What is P("no charging"=false)? How is it derived?
How would you go about determining "no charging" without having information about "fanbelt broken"?
Would the following be true:
P("no charging"=false) =
P("alternator broken"=true) * P("no charging"=true | "alternator broken"=true) + P("alternator broken"= false) * P("no charging"=true | "alternator broken"= false)
It's not possible
To calculate P("no charging") for the given BN, you are missing the prior for fanbelt broken. And also the CPT for no charging is underspecified, because no charging depends on fanbelt broken.
But you might want to
The best you can do with the information you have is simply ignore fanbelt broken. If the values for P( "charging" | "alternator broken") are obtained by taking the correct expectation over fanbelt broken, then the result is correct. If the latter is true this means that fanbelt broken is already eliminated (summed out), and it's influence is incorporated into the CPT for ´charging`.
I have run into a problem using PostGIS ST_DWithin using geometry points that has me completely stumped. Even though I'm using geometry points with SRID 3857 (see below), ST_DWithin interprets seems to interpret the third argument ( double precision distance_of_srid ) as DEGREES. Here's an example.
Using this table test_person_avg_lngs_lats:
Column | Type | Modifiers
---------------------+----------------------+-----------
avg_lng | double precision |
avg_lat | double precision |
person_avg_location | geometry(Point,3857) |
store_lng | double precision |
store_lat | double precision |
store_location | geometry(Point,3857) |
and the following query:
SELECT avg_lat, avg_lng, store_lng, store_lat,
ST_Distance_Spheroid(person_avg_location, store_location, CAST('SPHEROID[\"WGS 84\",6378137,298.257223563,AUTHORITY[\"EPSG","7030\"]]' AS spheroid))/1000 AS distance,
ST_DWithin(person_avg_location, store_location, 1) AS dwithin
FROM test_person_avg_lngs_lats
WHERE ST_DWithin(person_avg_location, store_location, 1)
The query returns results that interpret the third argument to ST_DWithin as 1 DEGREE versus 1 METER, even though I'm using geometry points with SRID 3857 which I have confirmed, uses meters units. Whatever N I pass as the third argument to ST_DWithin, the results consistently return distances around N * 100 km ( ~ 66 miles ). That's why I'm assuming ST_DWithin is interpreting it as 1 DEGREE.
Here is a sample result that should be interpreted as one meter (distance is in miles):
avg_lat avg_lng store_lng store_lat distance dwithin
43.3275959623, -71.1169553872, -71.0626, 42.3291, 68.9794023576, true
This is the closest I've come to something on the subject: ST_DWithin takes parameter as degree , not meters , why?
Any ideas as to what might be causing this, or what I might look for to move forward in analyzing the problem?
I'm using:
postgis_full_version
------------------------------------------------------------------
POSTGIS="2.0.0 r9605" GEOS="3.3.3-CAPI-1.7.4" PROJ="Rel. 4.8.0, 6 March 2012" GDAL="GDAL 1.9.2, released 2012/10/08" LIBXML="2.7.8" LIBJSON="UNKNOWN" RASTER
(1 row)
version
-------------------------------------------------------------
PostgreSQL 9.1.9 on x86_64-unknown-linux-gnu, compiled by gcc (Ubuntu/Linaro 4.6.3-1ubuntu5) 4.6.3, 64-bit
(1 row)
Obviously, distance_of_srid uses the same distance units of the SRID, which is typically either degrees or meters. With the geometry type, distances are calculated on a flat Cartesian plane using maths familiar to most high school students. The units are not interpreted for the geometry type.
However, this assumes that the data are actually projected with the correct SRID. If you mix a projected SRID like 3857 with lat/long coordinates expressed as degrees than you will get unexplainable garbage. Review how you populated the person_avg_location and store_location columns, because I'm 99.9% certain that there was an error there.
In C , how do I make 1200 / 500 = 3.
I'm doing a homework assignment.
Shipping Calculator: Speedy Shipping company will ship your package based on how much it weighs and how far you are sending the package. They will only ship small packages up to 10 pounds. You need to have a program that will help you determine how much they will charge. The charges are based on each 500 miles shipped. They are not pro-rated, i.e., 600 miles is the same charge as 900 miles.
Here is the table they gave you:
Package Weight--------------------------Rate per 500 miles shipped
2 pounds or less------------------------$1.50
More than 2 but not more than 6---------$3.70
More than 6 but not more than 10--------$5.25
Here is one test case.
Test Case Data:
Weight: 5.6 pounds
Miles: 1200 miles
Expected results:
Your shipping charge is $11.10
My answer keeps coming out to 7.40
Are you trying to round up? Before dividing, you could add 499 to the number that is being divided.
(0 + 499) / 500 -> 0
(1 + 499) / 500 -> 1
(1200 + 499) / 500 -> 3
This will round up.
Say you want to get a ceiling division a by b (in your example a = 1200 b = 500).
You can do it in integer arithmetic like this.
result = (a + b - 1) / b;
Or you could use floating point numbers and do it like this (probably a bad idea)
result = (int) ceil( (double) a / b );
The thing is that as this is a homework, you could just make it up in small steps:
if( a % b == 0 ) {
result = a / b;
} else {
result = a / b + 1;
}
Another advantage of this code is that it actually doesn't overflow for too big as, but this is not relevant in this case, I guess.
I'd suggest using the mod and truncate functions. If mod comes out zero, it's fine, otherwise truncate and add 1.
You have to use the ceiling of the division. This will round the quotient up to the next integer.
So when you are trying to find the number of 500-mile increments, you have to round the quotient up to the next integer.
Alternatively, (and inefficiently), you could increment the number of miles by 1, until it is divisible by 500...that is, while ( (q = x_miles++%500) != 0 ) {} . Then multipy q by the rate to get your answer (That is also assuming you will have an integer number of miles).
You could also use the stdlib div function. This might be nice if you only wanted integer math and specifically wanted to avoid floating point math.
http://www.cplusplus.com/reference/clibrary/cstdlib/div/
#include <stdlib.h>
int foo(void)
{
div_t result = div(1200, 500);
return result.quot + (0 < result.rem);
}
[EDIT1]
From your code you would implement this part as follows:
if ( weight <= 5.6 )
{
int multiplier = (int) miles / 500;
if( ((int)miles % 500) > 0)
multiplier++;
rate370 = (double)multiplier * 3.7;
printf("Your total cost : %.2lf\n", rate370);
}
[ORIGINAL]
In "integer land" 1200 / 3 should equal to 2.
for what it "seems" you want try this:
int multFiveHundreds = (int)totalWeight / 500;
if(multFiveHundreds % 500 > 0)
multFiveHundreds++;
I'm looking to create a db model of various units and their relation to each other. For instance, 36 inches = 3 feet = 1 yard = .9144 meters etc. This table would also store cups in ounces, pounds, kg, grams, cm and all sorts of measurements.
How do you do this? I was thinking about something like this:
Amount | Units | ConversionFactor | ConversionUnits
1 | foot | 12 | inches
1 | yard | 36 | inches
But frankly, this seems like a terrible idea. Trying to figure out how many feet in a yard would be very convoluted, and I don't think I could ever store all the conversions I need.
What other ideas are there? I know this is a solved problem. Thanks!
Store conversions to SI units, not to other non-metric units. Then you can convert between units in without needing to know the explicit conversion.
Unit | Class | Base Unit Multiplier
------------------------------------------------------
foot | length | 0.304800610
yard | length | 0.914401830
square foot | area | 0.092903040
...
So 14 feet in yards is:
14 feet * 0.304800610 = 4.26720854 meters
4.26720854 meters * 0.914401830⁻¹ = 4.66666667 yards
Pick a base unit for each dimension you are interested in (read that wiki page, it'll be useful). For example, if most of your data is in SI units, you would pick kilogram for mass, second for time, metre for distance, and so on. If most of your data is in US units, pick units from the US customary units, for example pound for mass, foot for length, second for time.
Then store, for each actual unit you want to be able to handle, the conversion factor to the dimensionally-appropriate base unit. So if you choose foot as your base unit of distance, store
Unit Dimension Factor
Foot Distance 1
Metre Distance 3.28084
Mile Distance 5280
To actually do a conversion, once you've checked that the dimensions match, simply multiply by the Factor of the source unit, and divide by the Factor of the destination unit. For example, to get from metres to miles, multiply by 3.28084, then divide by 5280.
CREATE TABLE UnitConversion
(
[FromUnit] NVARCHAR(100),
[ToUnit] NVARCHAR(100),
[FromOffset] DECIMAL(29,10),
[Multiplicand] DECIMAL(29,10),
[Denominator] DECIMAL(29,10),
[ToOffset] DECIMAL(29,10)
)
ToUnit = (FromUnit + FromOffset) * Multiplicand / Denominator + ToOffset
I think the original post's proposed schema is fine, apart from not including Class (as in Seth's answer) - you don't want to try to convert between pints and inches.
Converting between two units where neither of them is the conversion unit is simply achieved by retrieving both units' conversion records and dividing one factor by the other (eg. 36/12 = 3 feet in a yard).
If you are particularly concerned about accuracy, you could ensure that all units for a given class have entries for all other units in the same class - this strikes me as overkill, though.