I need to use the Kalman filter to fuse multi-sensors positions for gaussian measurement (for example 4 positions as the input of the filter and 1 position as output). It is possible to help me with some examples or tutorials because all the examples I found are related to the estimation of the positions?
OPTION 1
Weighted Avarage
In this case you don't need to implement a real Kalman Filter. You just can use the signal variances to calculate the weights and then calculate the weighted avarage of the inputs. The weights can be found as an inverse of the variances.
So if you have two signals S1 and S2 with variances V1 and V2, then the fused result would be
A fusion example can be seen on the next plot.
I simulated two signals. The variance of the second signal changes over the time. As long as it's smaller than the variance of the first signal the fused result is close to the second signal. It is not the case when the variance of the second signal is too high.
OPTION 2
Kalman Filter with Multiple Update Steps
The classical Kalman Filter uses prediction and update steps in a loop:
prediction
update
prediction
update
...
In your case you have 4 independent measurements, so you can use those readings after each other in separate update steps:
prediction
update 1
update 2
update 3
update 4
prediction
update 1
...
A very nice point is that the order of those updates does not matter! You can use updates 1,2,3,4 or 3,2,4,1. In both cases you should get the same fused output.
Compared to the first option you have following pros:
You have a variance propogation
You have the system noise matrix Q,
so you can control the smoothness of the fused output
Here is my matlab code:
function [] = main()
% time step
dt = 0.01;
t=(0:dt:2)';
n = numel(t);
%ground truth
signal = sin(t)+t;
% state matrix
X = zeros(2,1);
% covariance matrix
P = zeros(2,2);
% kalman filter output through the whole time
X_arr = zeros(n, 2);
% system noise
Q = [0.04 0;
0 1];
% transition matrix
F = [1 dt;
0 1];
% observation matrix
H = [1 0];
% variance of signal 1
s1_var = 0.08*ones(size(t));
s1 = generate_signal(signal, s1_var);
% variance of signal 2
s2_var = 0.01*(cos(8*t)+10*t);
s2 = generate_signal(signal, s2_var);
% variance of signal 3
s3_var = 0.02*(sin(2*t)+2);
s3 = generate_signal(signal, s3_var);
% variance of signal 4
s4_var = 0.06*ones(size(t));
s4 = generate_signal(signal, s4_var);
% fusion
for i = 1:n
if (i == 1)
[X, P] = init_kalman(X, s1(i, 1)); % initialize the state using the 1st sensor
else
[X, P] = prediction(X, P, Q, F);
[X, P] = update(X, P, s1(i, 1), s1(i, 2), H);
[X, P] = update(X, P, s2(i, 1), s2(i, 2), H);
[X, P] = update(X, P, s3(i, 1), s3(i, 2), H);
[X, P] = update(X, P, s4(i, 1), s4(i, 2), H);
end
X_arr(i, :) = X;
end
plot(t, signal, 'LineWidth', 4);
hold on;
plot(t, s1(:, 1), '--', 'LineWidth', 1);
plot(t, s2(:, 1), '--', 'LineWidth', 1);
plot(t, s3(:, 1), '--', 'LineWidth', 1);
plot(t, s4(:, 1), '--', 'LineWidth', 1);
plot(t, X_arr(:, 1), 'LineWidth', 4);
hold off;
grid on;
legend('Ground Truth', 'Sensor Input 1', 'Sensor Input 2', 'Sensor Input 3', 'Sensor Input 4', 'Fused Output');
end
function [s] = generate_signal(signal, var)
noise = randn(size(signal)).*sqrt(var);
s(:, 1) = signal + noise;
s(:, 2) = var;
end
function [X, P] = init_kalman(X, y)
X(1,1) = y;
X(2,1) = 0;
P = [100 0;
0 300];
end
function [X, P] = prediction(X, P, Q, F)
X = F*X;
P = F*P*F' + Q;
end
function [X, P] = update(X, P, y, R, H)
Inn = y - H*X;
S = H*P*H' + R;
K = P*H'/S;
X = X + K*Inn;
P = P - K*H*P;
end
And here is the result:
I have a 3x3 matrix used to linearly transform RGB colors. How can I convert the color space of an RGB image using that matrix? Should I loop through all the pixels and convert them one by one or does a faster way exist?
Assuming that your 3x3 matrix M is supposed to be multiplied with your pixel value p as follows:
new_p = M * p;
(with p a column vector), then you can transform your nxmx3 image matrix as follows:
m = 1920;
n = 1080;
img = rand(n, m, 3);
tmp = reshape(img, [], 3); % no copy made
tmp = tmp * M.';
new_img = reshape(tmp, n, m, 3);
Note that here we transformed M * p into p.' * M.', so that we don't need to transpose the image, which would require copying all its data to a new array. Transposing M is a lot cheaper than transposing tmp.
You can do the above in a single line, avoiding the need for the tmp array:
new_img = reshape(reshape(img, [], 3) * M.', n, m, 3);
I have a matrix with dimension NxM and one vector with length M. I need the smallest distance of all the distances between each row of the matrix and the single vector.
My approach was to create a second matrix using repmat and then calculate the distances between each row, but I am only getting the total distance:
matrix2 = repmat(vector, N, 1);
norm(matrix1 - matrix2, 2)
Let me show this with an example:
m = rand (5, 2); # Your matrix with reference points
v = rand (1, 2); # Your vector
tmp = bsxfun(#minus, m, v);
d = hypot (tmp(:,1), tmp(:,2)); # Difference between v and all rows in m
# Find samllest distance
[dmin, ix] = min (d)
# visualize it
scatter (m(:,1), m(:,2), 8, "red");
hold on
plot([v(1), m(ix, 1)], [v(2), m(ix, 2)], "-*")
hold off
print ("out.png")
The red bubbles are points in m (2D case) and the "*" one is v. A blue line connects v with the one from m, which ist nearest to.
I think on octave, you can use automatic broadcasting to get the distances without repmat-ing or using norm like so -
dist = sum((matrix1 - vector).^2,2)
On MATLAB, you can avoid repmat with bsxfun like this -
dist = sum(bsxfun(#minus,matrix1,vector).^2,2)
And, if you still like to continue with repmat, you can get the distances like so -
matrix2 = repmat(vector, N, 1);
dist = sum((matrix1 - matrix2).^2,2)
Finally, to get the smallest distance and the corresponding row index, you can use min -
[smallest_dist,minrow_ind] = min(dist)
Is there an algorithm that can calculate the digits of a repeating-decimal ratio without starting at the beginning?
I'm looking for a solution that doesn't use arbitrarily sized integers, since this should work for cases where the decimal expansion may be arbitrarily long.
For example, 33/59 expands to a repeating decimal with 58 digits. If I wanted to verify that, how could I calculate the digits starting at the 58th place?
Edited - with the ratio 2124679 / 2147483647, how to get the hundred digits in the 2147484600th through 2147484700th places.
OK, 3rd try's a charm :)
I can't believe I forgot about modular exponentiation.
So to steal/summarize from my 2nd answer, the nth digit of x/y is the 1st digit of (10n-1x mod y)/y = floor(10 * (10n-1x mod y) / y) mod 10.
The part that takes all the time is the 10n-1 mod y, but we can do that with fast (O(log n)) modular exponentiation. With this in place, it's not worth trying to do the cycle-finding algorithm.
However, you do need the ability to do (a * b mod y) where a and b are numbers that may be as large as y. (if y requires 32 bits, then you need to do 32x32 multiply and then 64-bit % 32-bit modulus, or you need an algorithm that circumvents this limitation. See my listing that follows, since I ran into this limitation with Javascript.)
So here's a new version.
function abmody(a,b,y)
{
var x = 0;
// binary fun here
while (a > 0)
{
if (a & 1)
x = (x + b) % y;
b = (2 * b) % y;
a >>>= 1;
}
return x;
}
function digits2(x,y,n1,n2)
{
// the nth digit of x/y = floor(10 * (10^(n-1)*x mod y) / y) mod 10.
var m = n1-1;
var A = 1, B = 10;
while (m > 0)
{
// loop invariant: 10^(n1-1) = A*(B^m) mod y
if (m & 1)
{
// A = (A * B) % y but javascript doesn't have enough sig. digits
A = abmody(A,B,y);
}
// B = (B * B) % y but javascript doesn't have enough sig. digits
B = abmody(B,B,y);
m >>>= 1;
}
x = x % y;
// A = (A * x) % y;
A = abmody(A,x,y);
var answer = "";
for (var i = n1; i <= n2; ++i)
{
var digit = Math.floor(10*A/y)%10;
answer += digit;
A = (A * 10) % y;
}
return answer;
}
(You'll note that the structures of abmody() and the modular exponentiation are the same; both are based on Russian peasant multiplication.)
And results:
js>digits2(2124679,214748367,214748300,214748400)
20513882650385881630475914166090026658968726872786883636698387559799232373208220950057329190307649696
js>digits2(122222,990000,100,110)
65656565656
js>digits2(1,7,1,7)
1428571
js>digits2(1,7,601,607)
1428571
js>digits2(2124679,2147483647,2147484600,2147484700)
04837181235122113132440537741612893408915444001981729642479554583541841517920532039329657349423345806
edit: (I'm leaving post here for posterity. But please don't upvote it anymore: it may be theoretically useful but it's not really practical. I have posted another answer which is much more useful from a practical point of view, doesn't require any factoring, and doesn't require the use of bignums.)
#Daniel Bruckner has the right approach, I think. (with a few additional twists required)
Maybe there's a simpler method, but the following will always work:
Let's use the examples q = x/y = 33/57820 and 44/65 in addition to 33/59, for reasons that may become clear shortly.
Step 1: Factor the denominator (specifically factor out 2's and 5's)
Write q = x/y = x/(2a25a5z). Factors of 2 and 5 in the denominator do not cause repeated decimals. So the remaining factor z is coprime to 10. In fact, the next step requires factoring z, so you might as well factor the whole thing.
Calculate a10 = max(a2, a5) which is the smallest exponent of 10 that is a multiple of the factors of 2 and 5 in y.
In our example 57820 = 2 * 2 * 5 * 7 * 7 * 59, so a2 = 2, a5 = 1, a10 = 2, z = 7 * 7 * 59 = 2891.
In our example 33/59, 59 is a prime and contains no factors of 2 or 5, so a2 = a5 = a10 = 0.
In our example 44/65, 65 = 5*13, and a2 = 0, a5 = a10 = 1.
Just for reference I found a good online factoring calculator here. (even does totients which is important for the next step)
Step 2: Use Euler's Theorem or Carmichael's Theorem.
What we want is a number n such that 10n - 1 is divisible by z, or in other words, 10n ≡ 1 mod z. Euler's function φ(z) and Carmichael's function λ(z) will both give you valid values for n, with λ(z) giving you the smaller number and φ(z) being perhaps a little easier to calculate. This isn't too hard, it just means factoring z and doing a little math.
φ(2891) = 7 * 6 * 58 = 2436
λ(2891) = lcm(7*6, 58) = 1218
This means that 102436 ≡ 101218 ≡ 1 (mod 2891).
For the simpler fraction 33/59, φ(59) = λ(59) = 58, so 1058 ≡ 1 (mod 59).
For 44/65 = 44/(5*13), φ(13) = λ(13) = 12.
So what? Well, the period of the repeating decimal must divide both φ(z) and λ(z), so they effectively give you upper bounds on the period of the repeating decimal.
Step 3: More number crunching
Let's use n = λ(z). If we subtract Q' = 10a10x/y from Q'' = 10(a10 + n)x/y, we get:
m = 10a10(10n - 1)x/y
which is an integer because 10a10 is a multiple of the factors of 2 and 5 of y, and 10n-1 is a multiple of the remaining factors of y.
What we've done here is to shift left the original number q by a10 places to get Q', and shift left q by a10 + n places to get Q'', which are repeating decimals, but the difference between them is an integer we can calculate.
Then we can rewrite x/y as m / 10a10 / (10n - 1).
Consider the example q = 44/65 = 44/(5*13)
a10 = 1, and λ(13) = 12, so Q' = 101q and Q'' = 1012+1q.
m = Q'' - Q' = (1012 - 1) * 101 * (44/65) = 153846153846*44 = 6769230769224
so q = 6769230769224 / 10 / (1012 - 1).
The other fractions 33/57820 and 33/59 lead to larger fractions.
Step 4: Find the nonrepeating and repeating decimal parts.
Notice that for k between 1 and 9, k/9 = 0.kkkkkkkkkkkkk...
Similarly note that a 2-digit number kl between 1 and 99, k/99 = 0.klklklklklkl...
This generalizes: for k-digit patterns abc...ij, this number abc...ij/(10k-1) = 0.abc...ijabc...ijabc...ij...
If you follow the pattern, you'll see that what we have to do is to take this (potentially) huge integer m we got in the previous step, and write it as m = s*(10n-1) + r, where 1 ≤ r < 10n-1.
This leads to the final answer:
s is the non-repeating part
r is the repeating part (zero-padded on the left if necessary to ensure that it is n digits)
with a10 =
0, the decimal point is between the
nonrepeating and repeating part; if
a10 > 0 then it is located
a10 places to the left of
the junction between s and r.
For 44/65, we get 6769230769224 = 6 * (1012-1) + 769230769230
s = 6, r = 769230769230, and 44/65 = 0.6769230769230 where the underline here designates the repeated part.
You can make the numbers smaller by finding the smallest value of n in step 2, by starting with the Carmichael function λ(z) and seeing if any of its factors lead to values of n such that 10n ≡ 1 (mod z).
update: For the curious, the Python interpeter seems to be the easiest way to calculate with bignums. (pow(x,y) calculates xy, and // and % are integer division and remainder, respectively.) Here's an example:
>>> N = pow(10,12)-1
>>> m = N*pow(10,1)*44//65
>>> m
6769230769224
>>> r=m%N
>>> r
769230769230
>>> s=m//N
>>> s
6
>>> 44/65
0.67692307692307696
>>> N = pow(10,58)-1
>>> m=N*33//59
>>> m
5593220338983050847457627118644067796610169491525423728813
>>> r=m%N
>>> r
5593220338983050847457627118644067796610169491525423728813
>>> s=m//N
>>> s
0
>>> 33/59
0.55932203389830504
>>> N = pow(10,1218)-1
>>> m = N*pow(10,2)*33//57820
>>> m
57073676928398478035281909373919059149083362158422691110342442061570390868211691
45624351435489450017295053614666205465236942234520927014873746108612936700103770
32168799723279142165340712556208924247665167762020062262193012798339674852992044
27533725354548599100657212037357315807679003804911795226565202352127291594603943
27222414389484607402282947077135939121411276374956762365963334486336907644413697
68246281563472846765824974057419578000691802144586648218609477689380837080594949
84434451746800415081286751988931165686613628502248356969906606710480802490487720
51193358699411968177101349014181943964026288481494292632307160152196471809062608
09408509166378415773088896575579384296091317883085437564856451054998270494638533
37945347630577654790729851262538913870632998962296783120027672085783465928744379
10757523348322379799377378069872016603251470079557246627464545140089934278796264
26841923209961950882047734347976478727084053960567277758561051539259771705292286
40608785887236250432376340366655136630923555863023175371843652715323417502594258
04219993081978554133517813905223106191629194050501556554825319958491871324801106
88343133863714977516430300933932895191975095122794880664130058803182289865098581
80560359737115185
>>> r=m%N
>>> r
57073676928398478035281909373919059149083362158422691110342442061570390868211691
45624351435489450017295053614666205465236942234520927014873746108612936700103770
32168799723279142165340712556208924247665167762020062262193012798339674852992044
27533725354548599100657212037357315807679003804911795226565202352127291594603943
27222414389484607402282947077135939121411276374956762365963334486336907644413697
68246281563472846765824974057419578000691802144586648218609477689380837080594949
84434451746800415081286751988931165686613628502248356969906606710480802490487720
51193358699411968177101349014181943964026288481494292632307160152196471809062608
09408509166378415773088896575579384296091317883085437564856451054998270494638533
37945347630577654790729851262538913870632998962296783120027672085783465928744379
10757523348322379799377378069872016603251470079557246627464545140089934278796264
26841923209961950882047734347976478727084053960567277758561051539259771705292286
40608785887236250432376340366655136630923555863023175371843652715323417502594258
04219993081978554133517813905223106191629194050501556554825319958491871324801106
88343133863714977516430300933932895191975095122794880664130058803182289865098581
80560359737115185
>>> s=m//N
>>> s
0
>>> 33/57820
0.00057073676928398479
with the overloaded Python % string operator usable for zero-padding, to see the full set of repeated digits:
>>> "%01218d" % r
'0570736769283984780352819093739190591490833621584226911103424420615703908682116
91456243514354894500172950536146662054652369422345209270148737461086129367001037
70321687997232791421653407125562089242476651677620200622621930127983396748529920
44275337253545485991006572120373573158076790038049117952265652023521272915946039
43272224143894846074022829470771359391214112763749567623659633344863369076444136
97682462815634728467658249740574195780006918021445866482186094776893808370805949
49844344517468004150812867519889311656866136285022483569699066067104808024904877
20511933586994119681771013490141819439640262884814942926323071601521964718090626
08094085091663784157730888965755793842960913178830854375648564510549982704946385
33379453476305776547907298512625389138706329989622967831200276720857834659287443
79107575233483223797993773780698720166032514700795572466274645451400899342787962
64268419232099619508820477343479764787270840539605672777585610515392597717052922
86406087858872362504323763403666551366309235558630231753718436527153234175025942
58042199930819785541335178139052231061916291940505015565548253199584918713248011
06883431338637149775164303009339328951919750951227948806641300588031822898650985
8180560359737115185'
As a general technique, rational fractions have a non-repeating part followed by a repeating part, like this:
nnn.xxxxxxxxrrrrrr
xxxxxxxx is the nonrepeating part and rrrrrr is the repeating part.
Determine the length of the nonrepeating part.
If the digit in question is in the nonrepeating part, then calculate it directly using division.
If the digit in question is in the repeating part, calculate its position within the repeating sequence (you now know the lengths of everything), and pick out the correct digit.
The above is a rough outline and would need more precision to implement in an actual algorithm, but it should get you started.
AHA! caffiend: your comment to my other (longer) answer (specifically "duplicate remainders") leads me to a very simple solution that is O(n) where n = the sum of the lengths of the nonrepeating + repeating parts, and requires only integer math with numbers between 0 and 10*y where y is the denominator.
Here's a Javascript function to get the nth digit to the right of the decimal point for the rational number x/y:
function digit(x,y,n)
{
if (n == 0)
return Math.floor(x/y)%10;
return digit(10*(x%y),y,n-1);
}
It's recursive rather than iterative, and is not smart enough to detect cycles (the 10000th digit of 1/3 is obviously 3, but this keeps on going until it reaches the 10000th iteration), but it works at least until the stack runs out of memory.
Basically this works because of two facts:
the nth digit of x/y is the (n-1)th digit of 10x/y (example: the 6th digit of 1/7 is the 5th digit of 10/7 is the 4th digit of 100/7 etc.)
the nth digit of x/y is the nth digit of (x%y)/y (example: the 5th digit of 10/7 is also the 5th digit of 3/7)
We can tweak this to be an iterative routine and combine it with Floyd's cycle-finding algorithm (which I learned as the "rho" method from a Martin Gardner column) to get something that shortcuts this approach.
Here's a javascript function that computes a solution with this approach:
function digit(x,y,n,returnstruct)
{
function kernel(x,y) { return 10*(x%y); }
var period = 0;
var x1 = x;
var x2 = x;
var i = 0;
while (n > 0)
{
n--;
i++;
x1 = kernel(x1,y); // iterate once
x2 = kernel(x2,y);
x2 = kernel(x2,y); // iterate twice
// have both 1x and 2x iterations reached the same state?
if (x1 == x2)
{
period = i;
n = n % period;
i = 0;
// start again in case the nonrepeating part gave us a
// multiple of the period rather than the period itself
}
}
var answer=Math.floor(x1/y);
if (returnstruct)
return {period: period, digit: answer,
toString: function()
{
return 'period='+this.period+',digit='+this.digit;
}};
else
return answer;
}
And an example of running the nth digit of 1/700:
js>1/700
0.0014285714285714286
js>n=10000000
10000000
js>rs=digit(1,700,n,true)
period=6,digit=4
js>n%6
4
js>rs=digit(1,700,4,true)
period=0,digit=4
Same thing for 33/59:
js>33/59
0.559322033898305
js>rs=digit(33,59,3,true)
period=0,digit=9
js>rs=digit(33,59,61,true)
period=58,digit=9
js>rs=digit(33,59,61+58,true)
period=58,digit=9
And 122222/990000 (long nonrepeating part):
js>122222/990000
0.12345656565656565
js>digit(122222,990000,5,true)
period=0,digit=5
js>digit(122222,990000,7,true)
period=6,digit=5
js>digit(122222,990000,9,true)
period=2,digit=5
js>digit(122222,990000,9999,true)
period=2,digit=5
js>digit(122222,990000,10000,true)
period=2,digit=6
Here's another function that finds a stretch of digits:
// find digits n1 through n2 of x/y
function digits(x,y,n1,n2,returnstruct)
{
function kernel(x,y) { return 10*(x%y); }
var period = 0;
var x1 = x;
var x2 = x;
var i = 0;
var answer='';
while (n2 >= 0)
{
// time to print out digits?
if (n1 <= 0)
answer = answer + Math.floor(x1/y);
n1--,n2--;
i++;
x1 = kernel(x1,y); // iterate once
x2 = kernel(x2,y);
x2 = kernel(x2,y); // iterate twice
// have both 1x and 2x iterations reached the same state?
if (x1 == x2)
{
period = i;
if (n1 > period)
{
var jumpahead = n1 - (n1 % period);
n1 -= jumpahead, n2 -= jumpahead;
}
i = 0;
// start again in case the nonrepeating part gave us a
// multiple of the period rather than the period itself
}
}
if (returnstruct)
return {period: period, digits: answer,
toString: function()
{
return 'period='+this.period+',digits='+this.digits;
}};
else
return answer;
}
I've included the results for your answer (assuming that Javascript #'s didn't overflow):
js>digit(1,7,1,7,true)
period=6,digits=1428571
js>digit(1,7,601,607,true)
period=6,digits=1428571
js>1/7
0.14285714285714285
js>digit(2124679,214748367,214748300,214748400,true)
period=1759780,digits=20513882650385881630475914166090026658968726872786883636698387559799232373208220950057329190307649696
js>digit(122222,990000,100,110,true)
period=2,digits=65656565656
Ad hoc I have no good idea. Maybe continued fractions can help. I am going to think a bit about it ...
UPDATE
From Fermat's little theorem and because 39 is prime the following holds. (= indicates congruence)
10^39 = 10 (39)
Because 10 is coprime to 39.
10^(39 - 1) = 1 (39)
10^38 - 1 = 0 (39)
[to be continued tomorow]
I was to tiered to recognize that 39 is not prime ... ^^ I am going to update and the answer in the next days and present the whole idea. Thanks for noting that 39 is not prime.
The short answer for a/b with a < b and an assumed period length p ...
calculate k = (10^p - 1) / b and verify that it is an integer, else a/b has not a period of p
calculate c = k * a
convert c to its decimal represenation and left pad it with zeros to a total length of p
the i-th digit after the decimal point is the (i mod p)-th digit of the paded decimal representation (i = 0 is the first digit after the decimal point - we are developers)
Example
a = 3
b = 7
p = 6
k = (10^6 - 1) / 7
= 142,857
c = 142,857 * 3
= 428,571
Padding is not required and we conclude.
3 ______
- = 0.428571
7