How would you implement the following in C or C++ with 6 and 12 data points?

Source code would be very helpful.

Table 2. Sign definition of sensor raw measurements

	Accelerometer (signed integer)
Stationary position	Ax	Ay	Az
Zb down	0	0	+1 g
Zb up	0	0	-1 g
Yb down	0	+1 g	0
Yb up	0	-1 g	0
Xb down	+1 g	0	0
Xb up	-1 g	0	0

Equation 1 1/ A SC A OS 1/ A SC A,, A OS m 1/ A SC A OS Z1 ACC ACC ACC AxT ACC ACC ACC 22 ACC 23 A 20 ACC ACC 32 ACC 33 Az ACC 30 where [A m] is the 3 x 3 misalignment matrix between the accelerometer sensing axes and the device body axes, A SCi (i 3x, y, z) is the sensitivity (or scale factor) and A OSi is the zero-g level (or offset). The goal of accelerometer calibration is to determine 12 parameters from ACC10 to ACC33, so that with any given raw measurements at arbitrary positions, the normalized values A Ay1 and Az1 can be obtained, resulting in: Equation 2 x1 Z1 Calibration can be performed at 6 stationary positions as shown in Table 2. Collect 5 to 10 seconds of accelerometer raw data with ODR 100 Hz at each position with known Ax1, Ay 1 and AZ1. Then apply the least square method to obtain the 12 accelerometer calibration parameters