If you were studying 3D rotation and Kalman filter to play with IMU of M5StickC, It seems that you can't escape from the procession, or it's easier to use the procession, it smells like that. I've googled even the procession beyond my memory ... It is troublesome to mess with the formula so as not to make a mistake, so I tried to make a program that outputs the product of the determinant containing the formula as it is, also as a study of python (laugh)
Postscript: I also tried to support operators (* (including scalar times), +,-). Calculator-like fixed calculations are displayed Only the product, the product with the scalar, and the sum / difference, but I tried it first. Addendum 2: It is difficult to find and remove unnecessary terms in a matrix mixed with 0s and 1s, so I tried to remove as much as possible and provide it.
##########\python>matcal.py
Matrix input and operator test
a,b,c[Enter](Matrixrow1)
d,e,f[Enter]....(Matrix row 2...)
to finish matrix,[Enter] or operator '*','+','-'
ex.for [a,b] [x] [ 12]
[c,d] * [y] + [-34]
a,b[Enter]
c,d[Enter]
*[Enter]
x[Enter]
y[Enter]
+
12[Enter]
-34[Enter]
[Enter]
Input section from here $ \ left (\ begin {array} {ccc} a & b \\ c & d \ end {array} \ right) \ left (\ begin {array} {ccc} x \\ y \ end { array} \ right) + \ left (\ begin {array} {ccc} 12 \\ -34 \ end {array} \ right) Put $ and $ \ left (\ begin {array} {ccc} (a) I'm getting * x + b * y) + 12 \\ (c * x + d * y) + (-34) \ end {array} \ right) $.
input Matrix data.
>a,b
>c,d
>*
[ a b ]
[ c d ]
>x
>y
>+
[ a*x+b*y ]
[ c*x+d*y ]
>12
>-34
>
[ (a*x+b*y)+12 ]
[ (c*x+d*y)+(-34) ]
Constant * matrix is also OK.
input Matrix data.
>mag
>*
[ mag ]
>a,b
>c,d
>
[ mag*a mag*b ]
[ mag*c mag*d ]
matcal.py
#!/usr/bin/env python3
#Calculate the product of the matrix as the string of the expression
import sys
import copy
def str_brackets(s):
if ((s.find("+")>=0) or (s.find("-")>=0)) and (s.find("(")!=0) :
# +/-If there is, enclose it in parentheses. If there is already parentheses, do not enclose it.
return "("+s+")"
else:
return s
def str_mul(x,y):
#The result of multiplying the expressions X and y in a string
#Empty string if either is 0
#If one is 1, the other is returned as is
#Unless it is zero, it will be the connection of the next section."+"Add
#Premise that the front and back spaces have been deleted
if x=="0" or y=="0":
return ""
elif x=="1":
return str_brackets(y)+"+"
elif y=="1":
return str_brackets(x)+"+"
else:
return str_brackets(x)+"*"+str_brackets(y)+"+"
def matrix_add(A,B,op):
Acol=len(A[0])
Arow=len(A)
Bcol=len(B[0])
Brow=len(B)
C=copy.deepcopy(A)
if Acol!=Bcol or Arow!=Brow:
print("Matrix add/sub matching error")
sys.exit()
for r in range(0,Arow):
for c in range(0,Acol):
x= A[r][c]
y= B[r][c]
if x=="0":
C[r][c] = y
elif y=="0":
C[r][c] = x
else:
C[r][c] = str_brackets(A[r][c]) + op + str_brackets(B[r][c])
return C
def sca_matrix_mul(k,M):
#Multi of scalar and matrix
R = copy.deepcopy(M)
if k=="0":
for r in range(0,len(M)):
for c in range(0,len(M[0])):
R[r][c]="0"
elif k!="1":
for r in range(0,len(M)):
for c in range(0,len(M[0])):
R[r][c]=k+"*"+str_brackets(M[r][c])
# k=="1"Then return R as it is
return R
def matrix_mul(A,B):
Acol=len(A[0])
Arow=len(A)
Bcol=len(B[0])
Brow=len(B)
if Acol==1 and Arow==1:
R = sca_matrix_mul(A[0][0],B)
elif Bcol==1 and Brow==1:
R = sca_matrix_mul(B[0][0],A)
elif Acol!=Brow:
print("Matrix mul. matching error")
sys.exit()
else:
R=[]
for r in range(0,Arow):
Rrow=[]
for c in range(0,Bcol):
s=""
for i in range(0,Acol):
s += str_mul(A[r][i],B[i][c])
if len(s)==0:
Rrow.append("0")
else:
Rrow.append(s[:-1])
R.append(Rrow)
return R
def matrix_print(A):
for r in range(0,len(A)):
s="["
for c in range(0,len(A[0])):
s += " {} ".format(A[r][c])
s += "]"
print(s)
def matrix_make(s):
# s="a11,a12,a13..// a21,a22,a23...// .... an1,an2,an3...ann" style
# ex: "a11,a12,a13//a21,a22,a23//a31,a32,a33"
# [a11,a12,a13]
# [a21,a22,a23]
# [a31,a32,a33]
Srow=s.split("//")
M=[]
for r in range(0,len(Srow)):
Scol=[x.strip() for x in Srow[r].split(",")]
M.append(Scol)
cnum_pre=len(M[0])
for r in range(1,len(M)):
cnum = len(M[r])
if cnum!=cnum_pre:
print("The number of columns is not uniform error")
sys.exit()
return M
def matrix_str_stdin():
s=""
operator=""
while True:
print(">",end='')
s_inp=input()
if s_inp=="+" or s_inp=="-" or s_inp=="*":
s=s[2:]
operator=s_inp
break
elif len(s_inp)==0 or s_inp=="=":
s=s[2:]
break
else:
s += "//{}".format(s_inp)
return matrix_make(s),operator
print ("\nMatrix input and operator test")
print ("a,b,c[Enter](Matrixrow1)")
print ("d,e,f[Enter]....(Matrix row 2...)")
print ("to finish matrix,[Enter] or operator '*','+','-'")
print ("ex.for [a,b] [x] [ 12]")
print (" [c,d] * [y] + [-34]")
print ("a,b[Enter]")
print ("c,d[Enter]")
print ("*[Enter]")
print ("x[Enter]")
print ("y[Enter]")
print ("+")
print ("12[Enter]")
print ("-34[Enter]")
print ("[Enter]")
print ("input Matrix data.")
(A,op2) = matrix_str_stdin()
while True:
matrix_print(A)
op=op2
if op=="*":
(B,op2) = matrix_str_stdin()
A = matrix_mul(A,B)
elif op=="+" or op=="-":
(B,op2) = matrix_str_stdin()
A = matrix_add(A,B,op)
else:
break
Recommended Posts