I am trying to implement extended Kalman filter for the estimation of position from GPS pseudoranges. I used the Taylor series to linearize the measurement equation, the code for this is given below.
function [H, R, Po] = getHandRMat(currentdata,estimatedRecPos,sigmaNot)
% Caluclate R and H for calculating Kalman Gain
R = getErrorCovMatObs(sigmaNot, currentdata);
Po = sqrt((currentdata(:,3)-estimatedRecPos(1)).^2 + ...
(currentdata(:,4)-estimatedRecPos(2)).^2 +...
(currentdata(:,5)-estimatedRecPos(3)).^2)+estimatedRecPos(4);
H = [(estimatedRecPos(1) - currentdata(:,3))./Po, ...
(estimatedRecPos(2) - currentdata(:,4))./Po, ...
(estimatedRecPos(3) - currentdata(:,5))./Po, ones(size(Po))];
end
The state vector consists of [x, y, z, cdt] of the receiver. currentdata has the satellite X, Y, Z coordinates and pseudoranges. estimatedRecPos is the state vector.
Problem
When I perform the update step stateVec = stateVec + K*(Z - (H*stateVec));, the value of Z - (H*stateVec) is extremely high, which isn't right. So I'm not sure where I am going wrong. I feel my design matrix H is correct as, I am using the same for Least sq estimation and that seems to work .
** Diff between Actual measurement and predicted measurement**
1.0e+07 *
2.5856
2.6063
2.6019
2.6899
2.6016
2.7021
2.6136
2.5983
2.5863
Any help is greatly appreciated.
Thanks
PS. the algorithm is implemented in Matlab
Update 1: Here is the algorithm presented in probabilistic robotics text book by Sebastian Thrun and Wolfram Brgard.
Does this mean that instead of using the design matrix H to convert the state vector to predicted measurement, I directly use the non-linear eq. directly?
Related
The Matlab has its built-in function deconv() which performs deconvolution perfectly. However, I was trying to make another simple implementation using the property of Toeplitz matrix in calculating convolution, but it seems to be not universal. Can anyone help me modify it, please?
function x = mdeconv(h, y)
%Deconvolution
%
hT = toeplitz([h(1) zeros(1,length(y)-length(h))], [h zeros(1,length(y)-length(h)) ]);
%x = linsolve(hT, y);
x = y/hT;
end
P.S. It can also be done using Fourier transform in signal processing, but my goal was to perform deconvolution with matrix operations.
Data x is input to an autoregreesive model (AR) model. The output of the AR model is corrupted with Additive White Gaussian Noise at SNR = 30 dB. The observations are denoted by noisy_y.
Let there be close estimates h_hat of the AR model (these are obtained from Least Squares estimation). I want to see how close the input obtained from deconvolution with h_hat and the measurements is to the known x.
My confusion is which variable to use for deconvolution -- clean y or noisy y?
Upon deconvolution, I should get x_hat. I am not sure if the correct way to perform deconvolution is using the noisy_y or using the y before adding noise. I have used the following code.
Can somebody please help in what is the correct method to plot x and x_hat.
Below is the plot of x vs x_hat. As can be seen, that these do not match. Where is my understand wrong? Please help.
The code is:
clear all
N = 200; %number of data points
a1=0.1650;
b1=-0.850;
h = [1 a1 b1]; %true coefficients
x = rand(1,N);
%%AR model
y = filter(1,h,x); %transmitted signal through AR channel
noisy_y = awgn(y,30,'measured');
hat_h= [1 0.133 0.653];
x_hat = filter(hat_h,1,noisy_y); %deconvolution
plot(1:50,x(1:50),'b');
hold on;
plot(1:50,x_hat(1:50),'-.rd');
A first issue is that the coefficients h of your AR model correspond to an unstable system since one of its poles is located outside the unit circle:
>> abs(roots(h))
ans =
1.00814
0.84314
Parameter estimation techniques are then quite likely to fail to converge given a diverging input sequence. Indeed, looking at the stated hat_h = [1 0.133 0.653] it is pretty clear that the parameter estimation did not converge anywhere near the actual coefficients. In your specific case you did not provide the code illustrating how you obtained hat_h (other than specifying that it was "obtained from Least Squares estimation"), so it isn't possible to further comment on what went wrong with your estimation.
That said, the standard formulation of Least Mean Squares (LMS) filters is given for an MA model. A common method for AR parameter estimation is to solve the Yule-Walker equations:
hat_h = aryule(noisy_y - mean(noisy_y), length(h)-1);
If we were to use this estimation method with the stable system defined by:
h = [1 -a1 -b1];
x = rand(1,N);
%%AR model
y = filter(1,h,x); %transmitted signal through AR channel
noisy_y = awgn(y,30,'measured');
hat_h = aryule(noisy_y - mean(noisy_y), length(h)-1);
x_hat = filter(hat_h,1,noisy_y); %deconvolution
The plot of x and x_hat would look like:
I have a question about Kalman filter. I am using Kalman filter for a state space model as following:
X(k+1) = A(k)x(k)+B(k)u(k)+w(k), w(k) ∼ N(0,Q)
Y(k) = C(K)x(k)+D(k)u(k)+v(k), v(k) ∼ N(0,R)
Which the state space matrixes (A(k),B(k),C(k),D(k)) are updated in each sampling time but Q and R matrixes are considered to be constant. The equations which calculate the Kalman gain (K(k)) and covariance P matrix (P(k)) are as following:
K(k) = (P(k)*C(k)' )/(R + C(k)*P(k)*C(k)');
Pxx(k) = (eye(n)-K(k)*C(k))*P(k)*(eye(n)-K(k)*C (k))'+K(k)*R*K(k)';%Joseph form
P(k) = A(k)*Pxx(k)*A(k)' + Q;
The problem that I face is related to stability of (A(k)-K(k)*C(k)). In some sampling times, the calculated Kalman gain can not stabilize the (A(k)-K(k)*C(k)) matrix and the eigenvalues of (A(k)-K(k)*C(k)) are outside of unit circle.
Could you please help me to figure out the reason for this problem? I am expecting that the Kalman filter gives me the gain which make the (A(k)-K(k)*C(k)) matrix stable with eigenvalues inside the unit circle.
This can happen when the system is unobservable. Check the rank of the observability matrix
O = [C(k)
C(k)A(k)
C(k)A(k)^2
...
C(k)A(k)^(n-1)]
where n is the dimension of the state space (i.e., A(k) is n X n matrix). If the row rank of O is less than n, then you have the problem.
In matlab, O = obsv(A,C) will compute the observability matrix and rank(O) will give its rank.
Hope this helps.
I have a problem when calculate discrete Fourier transform in MATLAB, apparently get the right result but when plot the amplitude of the frequencies obtained you can see values very close to zero which should be exactly zero. I use my own implementation:
function [y] = Discrete_Fourier_Transform(x)
N=length(x);
y=zeros(1,N);
for k = 1:N
for n = 1:N
y(k) = y(k) + x(n)*exp( -1j*2*pi*(n-1)*(k-1)/N );
end;
end;
end
I know it's better to use fft of MATLAB, but I need to use my own implementation as it is for college.
The code I used to generate the square wave:
x = [ones(1,8), -ones(1,8)];
for i=1:63
x = [x, ones(1,8), -ones(1,8)];
end
MATLAB version: R2013a(8.1.0.604) 64 bits
I have tried everything that has happened to me but I do not have much experience using MATLAB and I have not found information relevant to this issue in forums. I hope someone can help me.
Thanks in advance.
This will be a numerical problem. The values are in the range of 1e-15, while the DFT of your signal has values in the range of 1e+02. Most likely this won't lead to any errors when doing further processing. You can calculate the total squared error between your DFT and the MATLAB fft function by
y = fft(x);
yh = Discrete_Fourier_Transform(x);
sum(abs(yh - y).^2)
ans =
3.1327e-20
which is basically zero. I would therefore conclude: your DFT function works just fine.
Just one small remark: You can easily vectorize the DFT.
n = 0:1:N-1;
k = 0:1:N-1;
y = exp(-1j*2*pi/N * n'*k) * x(:);
With n'*k you create a matrix with all combinations of n and k. You then take the exp(...) of each of those matrix elements. With x(:) you make sure x is a column vector, so you can do the matrix multiplication (...)*x which automatically sums over all k's. Actually, I just notice, this is exactly the well-known matrix form of the DFT.
I have some data points to which I need to fit an exponential curve of the form
y = B * exp(A/x)
(without the help of Curve Fitting Toolbox).
What I have tried so far to linearize the model by applying log, which results in
log(y/B) = A/x
log(y) = A/x + log(B)
I can then write it in the form
Y = AX + B
Now, if I neglect B, then I am able to solve it with
A = pseudoinverse (X) * Y
but I am stuck with values of B...
Fitting a curve of the form
y = b * exp(a / x)
to some data points (xi, yi) in the least-squares sense is difficult. You cannot use linear least-squares for that, because the model parameters (a and b) do not appear in an affine manner in the equation. Unless you're ready to use some nonlinear-least-squares method, an alternative approach is to modify the optimization problem so that the modified problem can be solved using linear least squares (this process is sometimes called "data linearization"). Let's do that.
Under the assumption that b and the yi's be positive, you can apply the natural logarithm to both sides of the equations:
log(y) = log(b) + a / x
or
a / x + log(b) = log(y)
By introducing a new parameter b2, defined as log(b), it becomes evident that parameters a and b2 appear in a linear (affine, really) manner in the new equation:
a / x + b2 = log(y)
Therefore, you can compute the optimal values of those parameters using least squares; all you have left to do is construct the right linear system and then solve it using MATLAB's backslash operator:
A = [1 ./ x, ones(size(x))];
B = log(y);
params_ls = A \ B;
(I'm assuming x and y are column vectors, here.)
Then, the optimal values (in the least-squares sense) for the modified problem are given by:
a_ls = params_ls(1);
b_ls = exp(params_ls(2));
Although those values are not, in general, optimal for the original problem, they are often "good enough" in practice. If needed, you can always use them as initial guesses for some iterative nonlinear-least-squares method.
Doing the log transform then using linear regression should do it. Wikipedia has a nice section on how to do this:
http://en.wikipedia.org/wiki/Linear_least_squares_%28mathematics%29#The_general_problem
%MATLAB code for finding the best fit line using least squares method
x=input('enter a') %input in the form of matrix, rows contain points
a=[1,x(1,1);1,x(2,1);1,x(3,1)] %forming A of Ax=b
b=[x(1,2);x(2,2);x(3,2)] %forming b of Ax=b
yy=inv(transpose(a)*a)*transpose(a)*b %computing projection of matrix A on b, giving x
%plotting the best fit line
xx=linspace(1,10,50);
y=yy(1)+yy(2)*xx;
plot(xx,y)
%plotting the points(data) for which we found the best fit line
hold on
plot(x(2,1),x(2,2),'x')
hold on
plot(x(1,1),x(1,2),'x')
hold on
plot(x(3,1),x(3,2),'x')
hold off
I'm sure the code can be cleaned up, but that's the gist of it.