linear algebra - Applying PID Controller for trajectory planning algorithm

I am developing a path planning algorithm for a Segway robot. After running the algorithm path to the goal is obtained in form of (x, y, theta) coordinates. The hardware and sensor noises causing much trouble in following the path and error is accumulating. I thought of applying PID Control. I need to set the steering angle to the direction of movement by calculating the cross track error. 1) How to calculate the CTE with original co-ordinate (x1, y1, theta1) and current position (x1', y1', theta1')?2) How to select values for Kp, Ki and Kd for...Read more

linear algebra - FMA instruction showing up as three packed double operations?

I'm analyzing a piece of linear algebra code which is calling intrinsics directly, e.g.v_dot0 = _mm256_fmadd_pd( v_x0, v_y0, v_dot0 );My test script computes the dot product of two double precision vectors of length 4 (so only one call to _mm256_fmadd_pd needed), repeated 1 billion times. When I count the number of operations with perf I get something as follows:Performance counter stats for './main': 0 r5380c7 (skl::FP_ARITH:512B_PACKED_SINGLE) (49.99%) 0 r...Read more

linear algebra - How to calculate distance from center of a hypersphere (whitened PCA)

Lets say that I have calculated the principal components of a reference data set including whitening. The transformation matrix created from the principal component vectors is then applies to a test data set, projecting it onto the subspace of PCs. Now, I should be able to measure the distance of each of the test data vectors from the center of the PC hypersphere by simply adding up the coefficients of each column. Is this correct? Applying this transformation to my reference data gives a length of zero for all columns, and the length of the ve...Read more

linear algebra - Calculating error in PCA

I have a question about a result which I did not expect when doing PCA. I have successfully calculated the principal components using reference data, and then as a check to ensure that what's going on is what I think is going on, I've projected the reference data onto the entire basis of its eigenfucntions (kept all components) and then transformed back, (this is in python, so it's followed by ref_data_transform =pca.transform.(ref_data) followed by pca.inverse_transform(ref_data_transform) I get the exact same data. This is n...Read more

linear algebra - ambiguity when decomposing pitch (yaxis rotation) from rotation matrix

I am given a 4x4 pose of a robot where the z axis is facing forward, x-axis is facing east (right) and the y axis is point downwards.Now, to extract the heading of the robot, I used the following functionvoid inline mat2xyh(Matrix4f& pose, float &x, float &y, float &heading){ heading = atan2(-pose(2, 0), sqrt(pose(2, 1) * pose(2, 1) + pose(2,2) * pose(2,2)) ); x = pose(0, 3); y = pose(2, 3); };To validate, I setup the code below. When I examined the decomposed matrix angle and the angle i used to construct the rotatio...Read more

linear algebra - Output of this strange loop related to matrices

Let us consider the following pseudocode:int n=n;int A[][]scanf(A[][],%d);for i=1:n;i++{ x=A[i][i] for j=1:n;j++ { if x<A[i][j] a=x; x=A[i][j]; A[i][i]=x; A[i][j]=a; return A[][]I am fumbling on this pseudo code.the question, I think is just that the diagonal entries are compared and exchanged for the greatest entries. But, will the output depend on the entries of the matrix or will be independent of it is my main question. Specifically, is there any general formula for the output? Is it dependent on the type of matri...Read more

linear algebra - Matrix Inversion Methods

When one has a problem of a matrix inverse multiplication with a vector, as such: one can take a Cholesky Decomposition of A and backsubstitute b to find the resulting vector x. However, a matrix inverse is sometimes needed when the problem is not formulated as above. My question is what is the best way to handle such a situation. Below, I have compared various ways(using numpy) to invert a positive definite matrix:Firstly, generate the matrix:>>> A = np.random.rand(5,5)>>> Aarray([[ 0.13516074, 0.2532381 , 0.61169708, 0.99...Read more

linear algebra - Graceful Underflow

I have been searching about this for so long, but i am not able to understand what this question means.Question:Write a program in any language to determine how your computer handles graceful underflow.I understand that a overflow condition is something like this:if an integer can store a maximum value of x and if we assign a value of x+1, the value x+1 will be converted to the the lowest value the integer can hold. I understand that underflow is just the reverse. How does it stand from High performance scientific computing / Linear algebra p...Read more

linear algebra - Assigning a sparse matrix in Eigen

I read through this page on assigning a sparse matrix. Unfortunately, I do not understand it. Can anyone help me out with an example? For instance, how should I assign the following 10 by 8 sparse matrix in Eigen?A(1,2) = 3; A(1,4) = 10; A(2,1) = 9; A(2,8) = 9; A(4,7) = 11; A(5,3) = -23; A(7,6) = 21; A(9,3) = 45; A(10,7) = 76;...Read more

linear algebra - Lapack Orthonormalization Function for Rectangular Matrix

I was wondering if there was a function in Lapack for orthonormalizing the columns of a very tall and skinny matrix. A similar previous question asked this question, presumably in the context of a square matrix. My setting is as follows: I have an M by N matrix A that I am trying to orthonormalize the columns of.So, my first thought was to do a qr decomposition. The functions for doing a qr decomposition in Lapack seem to be dgeqrf and dormqr. Great. However, my problem is as follows: my matrix A is so tall, that I don't want to actually comput...Read more

linear algebra - Controlling orientation using a quaternion

To control a robotic arm, I have a controller with 6 dimensions (x, y, z position, and roll, pitch, yaw rotation). I am using a position (x, y, z) and quaternion (x, y, z, w) to represent the desired position and orientation of the robot's gripper. I am using some libraries that people might not be familiar with (namely, geometry_msgs from ROS), so here is what I'm doing in pseudocode:while(true): new_position = last_position + (joy.x, joy.y, joy.z) new_quaternion = last_quaternion * quaternionFromRPY(joy.roll, joy.pitch, joy.yaw) // (Comput...Read more